From 32268bf8fe5715d9426fd8aa02e60ba92687d062 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 19:07:01 +0900 Subject: [PATCH 01/33] Add pyrois library --- .../scripts/pyrois/.gitignore | 1 + .../scripts/pyrois/HRI_Engine_client.py | 129 ++++++++++ .../scripts/pyrois/HRI_Engine_example.py | 205 ++++++++++++++++ .../scripts/pyrois/Person_Detection.py | 134 ++++++++++ .../scripts/pyrois/Person_Detection_client.py | 104 ++++++++ .../scripts/pyrois/RoIS_Common.py | 67 +++++ .../scripts/pyrois/RoIS_HRI.py | 142 +++++++++++ .../scripts/pyrois/RoIS_Service.py | 55 +++++ .../Service_Application_Base_example.py | 100 ++++++++ .../scripts/pyrois/Service_Application_IF.py | 81 ++++++ .../scripts/pyrois/__init__.py | 0 .../scripts/pyrois/unittest.py | 231 ++++++++++++++++++ 12 files changed, 1249 insertions(+) create mode 100644 navigaion_signal_sender/scripts/pyrois/.gitignore create mode 100644 navigaion_signal_sender/scripts/pyrois/HRI_Engine_client.py create mode 100644 navigaion_signal_sender/scripts/pyrois/HRI_Engine_example.py create mode 100644 navigaion_signal_sender/scripts/pyrois/Person_Detection.py create mode 100644 navigaion_signal_sender/scripts/pyrois/Person_Detection_client.py create mode 100644 navigaion_signal_sender/scripts/pyrois/RoIS_Common.py create mode 100644 navigaion_signal_sender/scripts/pyrois/RoIS_HRI.py create mode 100644 navigaion_signal_sender/scripts/pyrois/RoIS_Service.py create mode 100644 navigaion_signal_sender/scripts/pyrois/Service_Application_Base_example.py create mode 100644 navigaion_signal_sender/scripts/pyrois/Service_Application_IF.py create mode 100644 navigaion_signal_sender/scripts/pyrois/__init__.py create mode 100644 navigaion_signal_sender/scripts/pyrois/unittest.py diff --git a/navigaion_signal_sender/scripts/pyrois/.gitignore b/navigaion_signal_sender/scripts/pyrois/.gitignore new file mode 100644 index 00000000..bee8a64b --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/.gitignore @@ -0,0 +1 @@ +__pycache__ diff --git a/navigaion_signal_sender/scripts/pyrois/HRI_Engine_client.py b/navigaion_signal_sender/scripts/pyrois/HRI_Engine_client.py new file mode 100644 index 00000000..6c79f528 --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/HRI_Engine_client.py @@ -0,0 +1,129 @@ +# 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 xmlrpc.client + +from pyrois import RoIS_HRI + + +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): + (s, command_id) = self._proxy.set_parameter(component_ref, parameters) + status = RoIS_HRI.ReturnCode_t(s) + 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): + """IF + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + +if __name__ == "__main__": + pass diff --git a/navigaion_signal_sender/scripts/pyrois/HRI_Engine_example.py b/navigaion_signal_sender/scripts/pyrois/HRI_Engine_example.py new file mode 100644 index 00000000..7b6770a6 --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/HRI_Engine_example.py @@ -0,0 +1,205 @@ +# HRI_Engine_example.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 + +"""HRI_Engine_example +""" + +from __future__ import print_function + +import os +import sys + +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) + + +class IF(SystemIF, CommandIF, QueryIF, EventIF): + """IF + """ + pass + + +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 MyHRIE: + """IF_Server + """ + def __init__(self): + self.state = False + + +def test_engine(port): + """test_engine + """ + IF_server(port).run(IF(MyHRIE())) + + +if __name__ == "__main__": + pass diff --git a/navigaion_signal_sender/scripts/pyrois/Person_Detection.py b/navigaion_signal_sender/scripts/pyrois/Person_Detection.py new file mode 100644 index 00000000..790033af --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/Person_Detection.py @@ -0,0 +1,134 @@ +# Person_Detection.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +"""Person_Detection +""" + +import sys +# import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import 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 Command(RoIS_Common.Command): + """Command + """ + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + def poll_event(self): + """poll_event + """ + msg = self.event_queue.get() + return msg + + def person_detected(self, timestamp, number): + """person_detected + """ + msg = xmlrpc.client.dumps((timestamp, number), 'person_detected') + self.event_queue.put(msg) + + +class Person_Detection(Event, Command, Query): + """Person_Detection + """ + pass + + +class component: + """component + """ + def __init__(self): + self._state = False + + +def event_dispatch(pd): + """event_dispatch + """ + pd.person_detected(datetime.now().isoformat(), 1) + time.sleep(0.1) + pd.person_detected(datetime.now().isoformat(), 1) + + +def example_pd(port): + """example_pd + """ + c = component() + pd = Person_Detection(c) + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(pd,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(pd) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + example_pd(8000) diff --git a/navigaion_signal_sender/scripts/pyrois/Person_Detection_client.py b/navigaion_signal_sender/scripts/pyrois/Person_Detection_client.py new file mode 100644 index 00000000..77f4af4b --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/Person_Detection_client.py @@ -0,0 +1,104 @@ +# Person_Detection_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 + +"""Person_Detection_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + (status, c_status) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + # 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 Person_Detection_Client(Command, Query, Event): + """Person_Detection_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() diff --git a/navigaion_signal_sender/scripts/pyrois/RoIS_Common.py b/navigaion_signal_sender/scripts/pyrois/RoIS_Common.py new file mode 100644 index 00000000..7cacd5aa --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/RoIS_Common.py @@ -0,0 +1,67 @@ +# RoIS_Common.py +# +# Copyright 2017 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component +""" +RoIS_Common +""" +import abc +import enum + +class Component_Status(enum.Enum): + """Component_Status + """ + UNINITIALIZED = 0 + READY = 1 + BUSY = 2 + WARNING = 3 + ERROR = 4 + +class Command: + """Command + """ + __metaclass__ = abc.ABCMeta + @abc.abstractmethod + def start(self): + """ + start () -> status: RoIS_HRI.ReturnCode_t + """ + pass + @abc.abstractmethod + def stop(self): + """ + stop () -> status: RoIS_HRI.ReturnCode_t + """ + pass + @abc.abstractmethod + def suspend(self): + """ + suspend () -> status: RoIS_HRI.ReturnCode_t + """ + pass + @abc.abstractmethod + def resume(self): + """ + resume () -> status: RoIS_HRI.ReturnCode_t + """ + pass + +class Query: + """Quary + """ + __metaclass__ = abc.ABCMeta + @abc.abstractmethod + def component_status(self): + """ + component_status () -> (status: RoIS_HRI.ReturnCode_t, status: Component_Status) + """ + pass + +class Event: + """Event + """ + pass diff --git a/navigaion_signal_sender/scripts/pyrois/RoIS_HRI.py b/navigaion_signal_sender/scripts/pyrois/RoIS_HRI.py new file mode 100644 index 00000000..4f3b7536 --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/RoIS_HRI.py @@ -0,0 +1,142 @@ +# RoIS_HRI.py +# +# Copyright 2017 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# for python 3 + +"""RoIS_HRI +""" +import abc +import enum + +class ReturnCode_t(enum.Enum): + """ReturnCode_t + """ + OK = 1 + ERROR = 2 + BAD_PARAMETER = 3 + UNSUPPORTED = 4 + OUT_OF_RESOURCES = 5 + TIMEOUT = 6 + +class SystemIF: + """ + class SystemIF(object) + """ + __metaclass__ = abc.ABCMeta + @abc.abstractmethod + def connect(self): + """ + connect() -> ReturnCode_t: status + """ + pass + @abc.abstractmethod + def disconnect(self): + """ + disconnect() -> ReturnCode_t: status + """ + pass + @abc.abstractmethod + def get_profile(self, condition): + """ + get_profile(condition) -> (status, profile) + """ + pass + @abc.abstractmethod + def get_error_detail(self, error_id, condition): + """ + get_error_detail(error_id,condition) -> (status,results) + """ + pass + +class CommandIF: + """ + class CommandIF(object): + """ + __metaclass__ = abc.ABCMeta + @abc.abstractmethod + def search(self, condition): + """ + search(condition) -> (status, component_ref_list) + """ + pass + @abc.abstractmethod + def bind(self, component_ref): + """ + bind(component_ref) -> status + """ + pass + @abc.abstractmethod + def bind_any(self, condition): + """ + bind_any(condition) -> (status,component_ref_list) + """ + pass + @abc.abstractmethod + def release(self, component_ref): + """ + release(component_ref) -> status + """ + pass + @abc.abstractmethod + def get_parameter(self, component_ref): + """ + get_parameter(self,component_ref) -> (status,parameter_list) + """ + pass + @abc.abstractmethod + def set_parameter(self, component_ref, parameters): + """ + set_parameter(self, component_ref, parameters) -> (status,command_id) + """ + pass + @abc.abstractmethod + def execute(self, command_unit_list): + """ + execute(command_unit_list) -> status + """ + pass + @abc.abstractmethod + def get_command_result(self, command_id, condition): + """ + get_command_result(self, command_id, condition) -> (status,results) + """ + pass + +class QueryIF: + """ + class QueryIF(object): + """ + __metaclass__ = abc.ABCMeta + @abc.abstractmethod + def query(self, query_type, condition): + """ + query(self, query_type, condition) -> (status,results) + """ + pass + +class EventIF: + """ + class EventIF(object): + """ + __metaclass__ = abc.ABCMeta + @abc.abstractmethod + def subscribe(self, event_type, condition): + """ + subscribe(self, event_type, condition) -> (status,subscribe_id) + """ + pass + @abc.abstractmethod + def unsubscribe(self, subscribe_id): + """ + unsubscribe(self,subscribe_id) -> status + """ + pass + @abc.abstractmethod + def get_event_detail(self, event_id, condition): + """ + get_event_detail(self,event_id,condition) -> (status,results) + """ + pass diff --git a/navigaion_signal_sender/scripts/pyrois/RoIS_Service.py b/navigaion_signal_sender/scripts/pyrois/RoIS_Service.py new file mode 100644 index 00000000..7cd57e5c --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/RoIS_Service.py @@ -0,0 +1,55 @@ +# RoIS_Service.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 + +"""RoIS_Service +""" + +import abc +import enum + +class Completed_Status(enum.Enum): + """Completed_Status + """ + OK = 1 + ERROR = 2 + ABORT = 3 + OUT_OF_RESOURCES = 4 + TIMEOUT = 5 + +class ErrorType(enum.Enum): + """ErrorType + """ + ENGINE_INTERNAL_ERROR = 1 + COMPONENT_INTERNEL_ERROR = 2 + COMPONENT_NOT_RESPONDING = 3 + USER_DEFINE_ERROR = 4 + +class Service_Application_Base: + """ + class ServiceApplicationBase(object) + """ + __metaclass__ = abc.ABCMeta + @abc.abstractmethod + def completed(self, command_id, status): + """ + connect(command_id, status) : void + """ + pass + @abc.abstractmethod + def notify_error(self, error_id, error_type): + """ + notify_error(error_id, error_type) : void + """ + pass + @abc.abstractmethod + def notify_event(self, event_id, event_type, subscribe_id, expire): + """ + notify_event(self, event_id, event_type, subscribe_id, expire) : void + """ + pass diff --git a/navigaion_signal_sender/scripts/pyrois/Service_Application_Base_example.py b/navigaion_signal_sender/scripts/pyrois/Service_Application_Base_example.py new file mode 100644 index 00000000..0d350047 --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/Service_Application_Base_example.py @@ -0,0 +1,100 @@ +# Service_Application_Base_example.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 + +"""Service_Application_Base_example +""" + +import sys +import queue +import time +import threading + +from pyrois import RoIS_Service + +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 Service_Application_Base(RoIS_Service.Service_Application_Base): + """Service_Application_Base + """ + def __init__(self): + self.event_queue = queue.Queue() + + def poll_event(self): + """poll_event + """ + msg = self.event_queue.get() + return msg + + def completed(self, command_id, status): + msg = xmlrpc.client.dumps((command_id, status), 'completed') + self.event_queue.put(msg) + + def notify_error(self, error_id, error_type): + msg = xmlrpc.client.dumps((error_id, error_type), 'notify_error') + self.event_queue.put(msg) + + def notify_event(self, event_id, event_type, subscribe_id, expire): + msg = xmlrpc.client.dumps( + (event_id, event_type, subscribe_id, expire), 'notify_event') + self.event_queue.put(msg) + + +def event_dispatch(sa): + """event_dispatch + """ + sa.completed("0", RoIS_Service.Completed_Status.OK.value) + time.sleep(0.1) + sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) + time.sleep(0.2) + sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") + + +# def event_dispatch_long(sa): +# sa.completed("0", RoIS_Service.Completed_Status.OK.value) +# time.sleep(1) +# sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) +# time.sleep(60) +# sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") + +def example_sa(port): + """example_sa + """ + sa = Service_Application_Base() + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(sa,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(sa) + server.register_introspection_functions() + server.register_multicall_functions() + # print("server running") + server.serve_forever() + + +if __name__ == '__main__': + example_sa(8000) diff --git a/navigaion_signal_sender/scripts/pyrois/Service_Application_IF.py b/navigaion_signal_sender/scripts/pyrois/Service_Application_IF.py new file mode 100644 index 00000000..c1ca652d --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/Service_Application_IF.py @@ -0,0 +1,81 @@ +# Service_Application_IF.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 + +"""Service_Application_IF +""" + +import xmlrpc.client +import logging +import logging.handlers + +from pyrois import RoIS_Service + + +class Service_Application_IF(RoIS_Service.Service_Application_Base): + """Service_Application_IF + """ + def __init__(self, uri, logger=None): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + if logger is not None: + self.logger = logger + while True: + self.poll_event() + + def poll_event(self): + """poll_event + """ + # print("poll") + msg = self._proxy.poll_event() + (params, methodname) = xmlrpc.client.loads(msg) + #self.logger.debug('poll_event: '+methodname) + # print(params,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.logger.debug('received completed event' + + command_id + + RoIS_Service.Completed_Status(status).name) + + def notify_error(self, error_id, error_type): + self.logger.debug('received error event' + + error_id + + RoIS_Service.ErrorType(error_type).name) + + def notify_event(self, event_id, event_type, subscribe_id, expire): + self.logger.debug('received event' + + event_id + + event_type + + subscribe_id + + expire) + + +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__': + pass diff --git a/navigaion_signal_sender/scripts/pyrois/__init__.py b/navigaion_signal_sender/scripts/pyrois/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/navigaion_signal_sender/scripts/pyrois/unittest.py b/navigaion_signal_sender/scripts/pyrois/unittest.py new file mode 100644 index 00000000..b6dfeaec --- /dev/null +++ b/navigaion_signal_sender/scripts/pyrois/unittest.py @@ -0,0 +1,231 @@ +# unittest.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# for python 3 + +"""unittest +""" + +import time +from multiprocessing import Process, Queue +import unittest +import http +import xmlrpc.client +import sysconfig + +from pyrois import RoIS_HRI, RoIS_Common +from pyrois import Service_Application_IF, Service_Application_Base_example +from pyrois import HRI_Engine_client, HRI_Engine_example +from pyrois import Person_Detection_client, Person_Detection + + +class TestServericeApplicationIF(unittest.TestCase): + """TestServericeApplicationIF + """ + def setUp(self): + # start the server process + self.ps = Process( + target=Service_Application_Base_example.example_sa, args=(8000,)) + self.ps.start() + time.sleep(0.1) + + def test_IF(self): + """ test_IF + """ + + q = Queue() + # start the client process + pc = Process(target=Service_Application_IF.example_sa_IF, + args=("http://127.0.0.1:8000/", q)) + pc.start() + + time.sleep(1) + + num_events = 0 + while not q.empty(): + msg = q.get() + num_events += 1 + # print(msg.asctime,msg.name,msg.levelname,msg.message) + + if pc.is_alive(): + pc.terminate() + + return self.assertEqual(num_events, 3) + + def tearDown(self): + # terminate the server process + if self.ps.is_alive(): + self.ps.terminate() + while self.ps.is_alive(): + time.sleep(0.1) + + +class TestHRIEngineIF(unittest.TestCase): + """TestHRIEngineIF + """ + + def setUp(self): + # start the server process + self.ps = Process(target=HRI_Engine_example.test_engine, args=(8000,)) + self.ps.start() + time.sleep(0.1) + + def test_IF(self): + """test_IF + """ + + with xmlrpc.client.ServerProxy("http://127.0.0.1:8000/") as proxy: + a = HRI_Engine_client.SystemIF(proxy) + b = HRI_Engine_client.CommandIF(proxy) + + try: + res = [ + a.connect(), + a.connect(), + b.bind(''), + a.disconnect(), + a.disconnect(), + b.bind('')] + # print(res) + except xmlrpc.client.ProtocolError as err: + print(err) + + return self.assertEqual(res, + [ + RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.ERROR, + RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.ERROR, + RoIS_HRI.ReturnCode_t.OK]) + + def tearDown(self): + # terminate the server process + if self.ps.is_alive(): + self.ps.terminate() + time.sleep(0.1) + + +class TestHRIEngineIF_integrated(unittest.TestCase): + """TestHRIEngineIF_integrated + """ + def setUp(self): + # start the server process + self.ps = Process(target=HRI_Engine_example.test_engine, args=(8000,)) + self.ps.start() + time.sleep(0.1) + + def test_IF(self): + """test_IF + """ + + try: + a = HRI_Engine_client.IF("http://127.0.0.1:8000/") + res = [ + a.connect(), + a.connect(), + a.get_profile(""), + a.get_error_detail("0", ""), + + a.search(""), + a.bind(""), + a.bind_any(""), + a.release(""), + a.get_parameter(""), + a.set_parameter("", ""), + a.execute(""), + a.get_command_result("", ""), + + a.query("", ""), + + a.subscribe("", ""), + a.unsubscribe(""), + a.get_event_detail("", ""), + + a.disconnect(), + a.disconnect(), + a.bind('')] + # print(res) + except xmlrpc.client.ProtocolError as err: + print(err) + + return self.assertEqual(res, + [RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.ERROR, + (RoIS_HRI.ReturnCode_t.OK, "Unsupported"), + (RoIS_HRI.ReturnCode_t.OK, "None"), + + (RoIS_HRI.ReturnCode_t.OK, ["None"]), + RoIS_HRI.ReturnCode_t.OK, + (RoIS_HRI.ReturnCode_t.OK, ["None"]), + RoIS_HRI.ReturnCode_t.OK, + (RoIS_HRI.ReturnCode_t.OK, ["None"]), + (RoIS_HRI.ReturnCode_t.OK, "0"), + RoIS_HRI.ReturnCode_t.OK, + (RoIS_HRI.ReturnCode_t.OK, ["None"]), + + (RoIS_HRI.ReturnCode_t.OK, ["None"]), + + (RoIS_HRI.ReturnCode_t.OK, "0"), + RoIS_HRI.ReturnCode_t.OK, + (RoIS_HRI.ReturnCode_t.OK, ["None"]), + + RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.ERROR, + RoIS_HRI.ReturnCode_t.OK]) + + def tearDown(self): + # terminate the server process + if self.ps.is_alive(): + self.ps.terminate() + time.sleep(0.1) + + +class TestPD(unittest.TestCase): + """TestPD + """ + + def setUp(self): + # start the server process + self.ps = Process(target=Person_Detection.example_pd, args=(8000,)) + self.ps.start() + time.sleep(0.5) + + def test_IF(self): + """test_IF + """ + a = Person_Detection_client.Person_Detection_Client("http://127.0.0.1:8000/") + try: + res = [ + a.start(), + a.stop(), + a.suspend(), + a.resume(), + a.component_status(), + len(a.events)] + except xmlrpc.client.ProtocolError as err: + print(err) + + return self.assertEqual(res, + [ + RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.OK, + RoIS_HRI.ReturnCode_t.OK, + (RoIS_HRI.ReturnCode_t.OK, RoIS_Common.Component_Status.UNINITIALIZED), + 2]) + + def tearDown(self): + # terminate the server process + if self.ps.is_alive(): + self.ps.terminate() + time.sleep(0.1) + + +if __name__ == '__main__': + print("python", sysconfig.get_python_version()) + print("platform:", sysconfig.get_platform()) + unittest.main() -- GitLab From 051bcaa500c3fe066e60acb171858c51368f3dfc Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 19:07:46 +0900 Subject: [PATCH 02/33] Add goal_sender_node with rois --- .../scripts/goal_sender_node.py | 153 ++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100755 navigaion_signal_sender/scripts/goal_sender_node.py diff --git a/navigaion_signal_sender/scripts/goal_sender_node.py b/navigaion_signal_sender/scripts/goal_sender_node.py new file mode 100755 index 00000000..4bc31ad5 --- /dev/null +++ b/navigaion_signal_sender/scripts/goal_sender_node.py @@ -0,0 +1,153 @@ +#!/usr/bin/env python + +# goal_sender_node.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +import sys +# import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import 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): + pass + + +class Command(RoIS_Common.Command): + def __init__(self, c): + self._component = c + + def start(self): + print("Sending goal...") + self._component.send_goal() + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + +class Event(RoIS_Common.Event): + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + 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) + + +class GoalSender(Event, Command, Query): + pass + + +def event_dispatch(gs): + # gs.person_detected(datetime.now().isoformat(), 1) + time.sleep(0.1) + # gs.person_detected(datetime.now().isoformat(), 1) + + +# import rospy +# from goal_sender_ros import GoalSenderROS +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)) + + +def example_gs(port): + print("Starting node...") + component = GoalSenderROS() + gs = GoalSender(component) + print("Goalsender constructed") + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(gs,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(gs) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + rospy.init_node('goal_sender_node') + example_gs(8000) -- GitLab From ddc8ebf8b5173f29d1bd3f8f86913bb826e599c5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 19:08:23 +0900 Subject: [PATCH 03/33] Remove old goal-sender node --- .../scripts/nagigation_signal_sender.py | 42 ------------------- 1 file changed, 42 deletions(-) delete mode 100755 navigaion_signal_sender/scripts/nagigation_signal_sender.py diff --git a/navigaion_signal_sender/scripts/nagigation_signal_sender.py b/navigaion_signal_sender/scripts/nagigation_signal_sender.py deleted file mode 100755 index 0f24f5ac..00000000 --- a/navigaion_signal_sender/scripts/nagigation_signal_sender.py +++ /dev/null @@ -1,42 +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 - - -def main(): - rospy.init_node('navigation_signal_sender') - - action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - action_client.wait_for_server() - - pose = [(10.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0)] - goal = pose_to_goal(pose) - state = 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 __name__ == '__main__': - main() -- GitLab From aa38c81741b7ca2bef8eae11043d1e8c51b673eb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 19:08:42 +0900 Subject: [PATCH 04/33] Add goal-sender client and test script --- .../scripts/goal_sender_client.py | 104 ++++++++++++++++++ navigaion_signal_sender/scripts/test.py | 8 ++ 2 files changed, 112 insertions(+) create mode 100644 navigaion_signal_sender/scripts/goal_sender_client.py create mode 100644 navigaion_signal_sender/scripts/test.py diff --git a/navigaion_signal_sender/scripts/goal_sender_client.py b/navigaion_signal_sender/scripts/goal_sender_client.py new file mode 100644 index 00000000..239def84 --- /dev/null +++ b/navigaion_signal_sender/scripts/goal_sender_client.py @@ -0,0 +1,104 @@ +# goal_sender_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 + +"""goal_sender_client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + (status, c_status) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + # 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 __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() diff --git a/navigaion_signal_sender/scripts/test.py b/navigaion_signal_sender/scripts/test.py new file mode 100644 index 00000000..71b90752 --- /dev/null +++ b/navigaion_signal_sender/scripts/test.py @@ -0,0 +1,8 @@ +import time +from goal_sender_client import GoalSenderClient + +uri = "http://localhost:8000" +client = GoalSenderClient(uri) + +time.sleep(3) +print(client.start()) -- GitLab From 2599f0e3b7c43d21c15974be19121fd374e67d01 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 19:11:47 +0900 Subject: [PATCH 05/33] Modify root .gitignore to ignore __pycache__ and *.pyc --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 973bb23a..63967614 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,6 @@ CMakeLists.txt !*/CMakeLists.txt ./.catkin_workspace + +__pycache__ +*.pyc -- GitLab From 7a35cf3d39d9038de2aa6a2f970023390a44a6b1 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 19:13:21 +0900 Subject: [PATCH 06/33] Split file for GoalSenderROS class --- .../scripts/goal_sender_node.py | 39 +----------------- .../scripts/goal_sender_ros.py | 41 +++++++++++++++++++ 2 files changed, 42 insertions(+), 38 deletions(-) create mode 100644 navigaion_signal_sender/scripts/goal_sender_ros.py diff --git a/navigaion_signal_sender/scripts/goal_sender_node.py b/navigaion_signal_sender/scripts/goal_sender_node.py index 4bc31ad5..07c545cf 100755 --- a/navigaion_signal_sender/scripts/goal_sender_node.py +++ b/navigaion_signal_sender/scripts/goal_sender_node.py @@ -90,45 +90,8 @@ def event_dispatch(gs): # gs.person_detected(datetime.now().isoformat(), 1) -# import rospy -# from goal_sender_ros import GoalSenderROS 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)) - +from goal_sender_ros import GoalSenderROS def example_gs(port): print("Starting node...") diff --git a/navigaion_signal_sender/scripts/goal_sender_ros.py b/navigaion_signal_sender/scripts/goal_sender_ros.py new file mode 100644 index 00000000..15c47218 --- /dev/null +++ b/navigaion_signal_sender/scripts/goal_sender_ros.py @@ -0,0 +1,41 @@ +#/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('navigation_signal_sender') -- GitLab From f5ea6ff9e5ee4b41e227835b95a44f4c15083c3c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 16 Oct 2019 01:06:50 +0900 Subject: [PATCH 07/33] Rename package navigation_signal_sender => goal_sender --- .../CMakeLists.txt | 10 +++++----- {navigaion_signal_sender => goal_sender}/package.xml | 6 +++--- .../scripts/goal_sender_client.py | 0 .../scripts/goal_sender_node.py | 0 .../scripts/goal_sender_ros.py | 2 +- .../scripts/pyrois/.gitignore | 0 .../scripts/pyrois/HRI_Engine_client.py | 0 .../scripts/pyrois/HRI_Engine_example.py | 0 .../scripts/pyrois/Person_Detection.py | 0 .../scripts/pyrois/Person_Detection_client.py | 0 .../scripts/pyrois/RoIS_Common.py | 0 .../scripts/pyrois/RoIS_HRI.py | 0 .../scripts/pyrois/RoIS_Service.py | 0 .../scripts/pyrois/Service_Application_Base_example.py | 0 .../scripts/pyrois/Service_Application_IF.py | 0 .../scripts/pyrois/__init__.py | 0 .../scripts/pyrois/unittest.py | 0 .../scripts/test.py | 0 18 files changed, 9 insertions(+), 9 deletions(-) rename {navigaion_signal_sender => goal_sender}/CMakeLists.txt (96%) rename {navigaion_signal_sender => goal_sender}/package.xml (93%) rename {navigaion_signal_sender => goal_sender}/scripts/goal_sender_client.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/goal_sender_node.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/goal_sender_ros.py (96%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/.gitignore (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/HRI_Engine_client.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/HRI_Engine_example.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/Person_Detection.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/Person_Detection_client.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/RoIS_Common.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/RoIS_HRI.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/RoIS_Service.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/Service_Application_Base_example.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/Service_Application_IF.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/__init__.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/pyrois/unittest.py (100%) rename {navigaion_signal_sender => goal_sender}/scripts/test.py (100%) diff --git a/navigaion_signal_sender/CMakeLists.txt b/goal_sender/CMakeLists.txt similarity index 96% rename from navigaion_signal_sender/CMakeLists.txt rename to goal_sender/CMakeLists.txt index 5a27dc4c..5a822e32 100644 --- a/navigaion_signal_sender/CMakeLists.txt +++ b/goal_sender/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(navigaion_signal_sender) +project(goal_sender) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -104,7 +104,7 @@ find_package(catkin REQUIRED COMPONENTS ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES navigaion_signal_sender +# LIBRARIES goal_sender # CATKIN_DEPENDS actionlib move_base_msgs rospy # DEPENDS system_lib ) @@ -122,7 +122,7 @@ include_directories( ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/navigaion_signal_sender.cpp +# src/${PROJECT_NAME}/goal_sender.cpp # ) ## Add cmake target dependencies of the library @@ -133,7 +133,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/navigaion_signal_sender_node.cpp) +# add_executable(${PROJECT_NAME}_node src/goal_sender_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -197,7 +197,7 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_navigaion_signal_sender.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_goal_sender.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/navigaion_signal_sender/package.xml b/goal_sender/package.xml similarity index 93% rename from navigaion_signal_sender/package.xml rename to goal_sender/package.xml index b67968e2..d739126d 100644 --- a/navigaion_signal_sender/package.xml +++ b/goal_sender/package.xml @@ -1,8 +1,8 @@ - navigaion_signal_sender + goal_sender 0.0.0 - The navigaion_signal_sender package + The goal_sender package @@ -19,7 +19,7 @@ - + diff --git a/navigaion_signal_sender/scripts/goal_sender_client.py b/goal_sender/scripts/goal_sender_client.py similarity index 100% rename from navigaion_signal_sender/scripts/goal_sender_client.py rename to goal_sender/scripts/goal_sender_client.py diff --git a/navigaion_signal_sender/scripts/goal_sender_node.py b/goal_sender/scripts/goal_sender_node.py similarity index 100% rename from navigaion_signal_sender/scripts/goal_sender_node.py rename to goal_sender/scripts/goal_sender_node.py diff --git a/navigaion_signal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py similarity index 96% rename from navigaion_signal_sender/scripts/goal_sender_ros.py rename to goal_sender/scripts/goal_sender_ros.py index 15c47218..014bce3a 100644 --- a/navigaion_signal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -38,4 +38,4 @@ class GoalSenderROS(object): rospy.loginfo("state: {}".format(state)) # If you remove this comment out, it does not work. -# rospy.init_node('navigation_signal_sender') +# rospy.init_node('goal_sender') diff --git a/navigaion_signal_sender/scripts/pyrois/.gitignore b/goal_sender/scripts/pyrois/.gitignore similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/.gitignore rename to goal_sender/scripts/pyrois/.gitignore diff --git a/navigaion_signal_sender/scripts/pyrois/HRI_Engine_client.py b/goal_sender/scripts/pyrois/HRI_Engine_client.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/HRI_Engine_client.py rename to goal_sender/scripts/pyrois/HRI_Engine_client.py diff --git a/navigaion_signal_sender/scripts/pyrois/HRI_Engine_example.py b/goal_sender/scripts/pyrois/HRI_Engine_example.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/HRI_Engine_example.py rename to goal_sender/scripts/pyrois/HRI_Engine_example.py diff --git a/navigaion_signal_sender/scripts/pyrois/Person_Detection.py b/goal_sender/scripts/pyrois/Person_Detection.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/Person_Detection.py rename to goal_sender/scripts/pyrois/Person_Detection.py diff --git a/navigaion_signal_sender/scripts/pyrois/Person_Detection_client.py b/goal_sender/scripts/pyrois/Person_Detection_client.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/Person_Detection_client.py rename to goal_sender/scripts/pyrois/Person_Detection_client.py diff --git a/navigaion_signal_sender/scripts/pyrois/RoIS_Common.py b/goal_sender/scripts/pyrois/RoIS_Common.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/RoIS_Common.py rename to goal_sender/scripts/pyrois/RoIS_Common.py diff --git a/navigaion_signal_sender/scripts/pyrois/RoIS_HRI.py b/goal_sender/scripts/pyrois/RoIS_HRI.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/RoIS_HRI.py rename to goal_sender/scripts/pyrois/RoIS_HRI.py diff --git a/navigaion_signal_sender/scripts/pyrois/RoIS_Service.py b/goal_sender/scripts/pyrois/RoIS_Service.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/RoIS_Service.py rename to goal_sender/scripts/pyrois/RoIS_Service.py diff --git a/navigaion_signal_sender/scripts/pyrois/Service_Application_Base_example.py b/goal_sender/scripts/pyrois/Service_Application_Base_example.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/Service_Application_Base_example.py rename to goal_sender/scripts/pyrois/Service_Application_Base_example.py diff --git a/navigaion_signal_sender/scripts/pyrois/Service_Application_IF.py b/goal_sender/scripts/pyrois/Service_Application_IF.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/Service_Application_IF.py rename to goal_sender/scripts/pyrois/Service_Application_IF.py diff --git a/navigaion_signal_sender/scripts/pyrois/__init__.py b/goal_sender/scripts/pyrois/__init__.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/__init__.py rename to goal_sender/scripts/pyrois/__init__.py diff --git a/navigaion_signal_sender/scripts/pyrois/unittest.py b/goal_sender/scripts/pyrois/unittest.py similarity index 100% rename from navigaion_signal_sender/scripts/pyrois/unittest.py rename to goal_sender/scripts/pyrois/unittest.py diff --git a/navigaion_signal_sender/scripts/test.py b/goal_sender/scripts/test.py similarity index 100% rename from navigaion_signal_sender/scripts/test.py rename to goal_sender/scripts/test.py -- GitLab From 64ba5b28e1a33414d9bd0b3f1d5bf79496200bee Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 18 Oct 2019 16:00:08 +0900 Subject: [PATCH 08/33] Add plane HRI-engine for route-guidance --- goal_sender/scripts/route_guidance_engine.py | 205 +++++++++++++++++++ 1 file changed, 205 insertions(+) create mode 100644 goal_sender/scripts/route_guidance_engine.py diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py new file mode 100644 index 00000000..3da9e8e5 --- /dev/null +++ b/goal_sender/scripts/route_guidance_engine.py @@ -0,0 +1,205 @@ +# 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 + +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) + + +class IF(SystemIF, CommandIF, QueryIF, EventIF): + """IF + """ + pass + + +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) -- GitLab From 9a20437c14574df5d042aba3774bee2d6cf2309e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 18 Oct 2019 16:17:44 +0900 Subject: [PATCH 09/33] Include custom client into IF class --- goal_sender/scripts/route_guidance_engine.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index 3da9e8e5..49eaab04 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -165,10 +165,16 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) +from goal_sender_client import GoalSenderClient as NavClient + class IF(SystemIF, CommandIF, QueryIF, EventIF): """IF """ - pass + def __init__(self, Engine): + super().__init__(Engine) + self.compoent_clients = { + 'Navigation': NavClient('http://localhost:8001') + } class IF_server: -- GitLab From ca3c6c6083a44cb738dddf99fc7810ebda5eb58c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 18 Oct 2019 17:01:39 +0900 Subject: [PATCH 10/33] Modify GoalSenderROS to response request of 'get_state' --- goal_sender/scripts/goal_sender_ros.py | 32 ++++++++++++++++++-------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 014bce3a..7958e1d9 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -2,11 +2,13 @@ import rospy import actionlib -from actionlib_msgs.msg import GoalStatus from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 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 + def pose_to_goal(pose): goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' @@ -22,20 +24,32 @@ def pose_to_goal(pose): class GoalSenderROS(object): + # Table for conversion from GoalStatus to Component_Status + # http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html + GoalSenderState = { + g_state.PENDING: c_state.READY, + g_state.ACTIVE: c_state.BUSY, + g_state.PREEMPTED: c_state.READY, + g_state.SUCCEEDED: c_state.READY, + g_state.ABORTED: c_state.ERROR, + g_state.PREEMPTING: c_state.BUSY, + g_state.RECALLING: c_state.BUSY, + g_state.RECALLED: c_state.READY, + g_state.LOST: c_state.ERROR + } + def __init__(self): self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - # self.action_client.wait_for_server() + 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)) + self.action_client.send_goal(goal) + + def get_goalsender_state(self): + state = self.action_client.get_state() + return GoalSenderROS.GoalSenderState[state] # If you remove this comment out, it does not work. # rospy.init_node('goal_sender') -- GitLab From f470e8e9ac446122432ca6470cb4b53d951d7a7d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 22 Oct 2019 17:02:35 +0900 Subject: [PATCH 11/33] Add sample navigation component --- goal_sender/scripts/Navigation.py | 141 +++++++++++++++++++++++ goal_sender/scripts/Navigation_client.py | 107 +++++++++++++++++ 2 files changed, 248 insertions(+) create mode 100755 goal_sender/scripts/Navigation.py create mode 100644 goal_sender/scripts/Navigation_client.py diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py new file mode 100755 index 00000000..ff81f2e8 --- /dev/null +++ b/goal_sender/scripts/Navigation.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python + +# Navigation.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +"""Navigation +""" + +import sys +import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def set_parameter(self, target_position, time_limit, routing_policy): + self._component.Target_Position = target_position + self._component.Time_Limit = time_limit + self._component.Routing_Policy = routing_policy + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + def get_parameter(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return (status, self._component.Target_Position, self._component.Time_Limit, self._component.Routing_Policy) + + +class Event(RoIS_Common.Event): + """Event + """ + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + # def poll_event(self): + # """poll_event + # """ + # msg = self.event_queue.get() + # return msg + + +class Navigation(Event, Command, Query): + """Navigation + """ + def __init__(self, c): + super().__init__(c) + self._component = c + self._component.Target_Position = [""] + self._component.Time_Limit = 10 + self._component.Routing_Policy = "" + + +class component: + """component + """ + def __init__(self): + self._state = False + + +def event_dispatch(n): + """event_dispatch + """ + + +def example_n(port): + """example_n + """ + c = component() + n = Navigation(c) + + # start the timer to dispatch events + 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(n) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + example_n(8013) diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py new file mode 100644 index 00000000..b384eddc --- /dev/null +++ b/goal_sender/scripts/Navigation_client.py @@ -0,0 +1,107 @@ +# Navigation_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 + +"""Navigation_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def start(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.start()) + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) + return status + + def set_parameter(self, target_position, time_limit, routing_policy): + s = self._proxy.set_parameter(target_position, time_limit, routing_policy) + status = RoIS_HRI.ReturnCode_t(s) + return status + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def component_status(self): + (status, c_status) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, 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): + """Event + """ + def __init__(self, uri): + self._uri = uri + self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] + # if logger is not None: + # self.logger = logger + + # def start_th(self): + # self.th = threading.Thread(target=self.event_loop) + # self.th.start() + + # def event_loop(self): + # """event_loop + # """ + # while True: + # try: + # self.poll_event() + # except ConnectionRefusedError: + # break + + # def poll_event(self): + # """poll_event + # """ + # msg = self._e_proxy.poll_event() + # (params, methodname) = xmlrpc.client.loads(msg) + # #self.logger.debug('poll_event: '+methodname) + # print(params,methodname) + # if methodname == 'speech_recognized': + # self.speech_recognized(params[0], params[1]) + + +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() \ No newline at end of file -- GitLab From feeda2ff9fc51db9d570f089004aadcf81b6c0a5 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 22 Oct 2019 17:59:09 +0900 Subject: [PATCH 12/33] Include ros node into Navigation Component server --- goal_sender/scripts/Navigation.py | 65 ++++++-------------- goal_sender/scripts/route_guidance_engine.py | 2 +- 2 files changed, 21 insertions(+), 46 deletions(-) diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py index ff81f2e8..e3267d30 100755 --- a/goal_sender/scripts/Navigation.py +++ b/goal_sender/scripts/Navigation.py @@ -9,11 +9,8 @@ # For python3 # For HRI Component -"""Navigation -""" - import sys -import queue +# import queue import time from datetime import datetime import threading @@ -21,24 +18,22 @@ 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 Command(RoIS_Common.Command): - """Command - """ def __init__(self, c): self._component = c @@ -58,17 +53,8 @@ class Command(RoIS_Common.Command): status = RoIS_HRI.ReturnCode_t.OK.value return status - def set_parameter(self, target_position, time_limit, routing_policy): - self._component.Target_Position = target_position - self._component.Time_Limit = time_limit - self._component.Routing_Policy = routing_policy - status = RoIS_HRI.ReturnCode_t.OK.value - return status - class Query(RoIS_Common.Query): - """Query - """ def __init__(self, c): self._component = c @@ -76,54 +62,42 @@ class Query(RoIS_Common.Query): status = RoIS_HRI.ReturnCode_t.OK.value c_status = RoIS_Common.Component_Status.UNINITIALIZED.value return (status, c_status) - - def get_parameter(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return (status, self._component.Target_Position, self._component.Time_Limit, self._component.Routing_Policy) - + class Event(RoIS_Common.Event): - """Event - """ def __init__(self, c): self._component = c self.event_queue = queue.Queue() - # def poll_event(self): - # """poll_event - # """ - # msg = self.event_queue.get() - # return msg - + # def poll_event(self): + # msg = self.event_queue.get() + # return msg class Navigation(Event, Command, Query): - """Navigation - """ def __init__(self, c): - super().__init__(c) + super(Navigation, self).__init__(c) self._component = c self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" -class component: - """component - """ - def __init__(self): - self._state = False +def event_dispatch(n): + # n.person_detected(datetime.now().isoformat(), 1) + time.sleep(0.1) + # n.person_detected(datetime.now().isoformat(), 1) -def event_dispatch(n): - """event_dispatch - """ +class Component(object): + pass +import rospy def example_n(port): - """example_n - """ - c = component() - n = Navigation(c) + print("Starting node...") + component = Component() + n = Navigation(component) + print("Navigation constructed") # start the timer to dispatch events t = threading.Timer(0.1, event_dispatch, args=(n,)) @@ -138,4 +112,5 @@ def example_n(port): if __name__ == '__main__': + rospy.init_node('navigation_component') example_n(8013) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index 49eaab04..c5a92bb5 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -165,7 +165,7 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) -from goal_sender_client import GoalSenderClient as NavClient +from Navigation_client import Navigation_Client as NavClient class IF(SystemIF, CommandIF, QueryIF, EventIF): """IF -- GitLab From 446d01f69524c2a9e3d5417ce6f68ea5bffe5d16 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:06:17 +0900 Subject: [PATCH 13/33] Add set_parameter method to Navigation Component To send goal to move_base --- goal_sender/scripts/Navigation.py | 10 +++++++++- goal_sender/scripts/Navigation_client.py | 2 +- goal_sender/scripts/test.py | 9 +++++---- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py index e3267d30..976efeac 100755 --- a/goal_sender/scripts/Navigation.py +++ b/goal_sender/scripts/Navigation.py @@ -73,14 +73,22 @@ class Event(RoIS_Common.Event): # msg = self.event_queue.get() # return 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 = "" + def set_parameter(self, target_position, time_limit, routing_policy): + self._goal_sender.send_goal() + status = RoIS_HRI.ReturnCode_t.OK.value + return status + def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) @@ -113,4 +121,4 @@ def example_n(port): if __name__ == '__main__': rospy.init_node('navigation_component') - example_n(8013) + example_n(8001) diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py index b384eddc..3b27ea8d 100644 --- a/goal_sender/scripts/Navigation_client.py +++ b/goal_sender/scripts/Navigation_client.py @@ -104,4 +104,4 @@ class Navigation_Client(Command, Query, Event): self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - # self.start_th() \ No newline at end of file + # self.start_th() diff --git a/goal_sender/scripts/test.py b/goal_sender/scripts/test.py index 71b90752..73fdab26 100644 --- a/goal_sender/scripts/test.py +++ b/goal_sender/scripts/test.py @@ -1,8 +1,9 @@ import time -from goal_sender_client import GoalSenderClient +from Navigation_client import Navigation_Client -uri = "http://localhost:8000" -client = GoalSenderClient(uri) +uri = "http://localhost:8001" +client = Navigation_Client(uri) -time.sleep(3) print(client.start()) +time.sleep(3) +print(client.set_parameter("ahi", "chuni", "dwa")) -- GitLab From 415c7b574b395b8e736dc2d8618c5d2456f5853c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:09:42 +0900 Subject: [PATCH 14/33] Rename test script to test_component --- goal_sender/scripts/{test.py => test_component.py} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename goal_sender/scripts/{test.py => test_component.py} (100%) diff --git a/goal_sender/scripts/test.py b/goal_sender/scripts/test_component.py similarity index 100% rename from goal_sender/scripts/test.py rename to goal_sender/scripts/test_component.py -- GitLab From c8067f7ab1f0170e7ee1c2a87a79ffbd1d44ee98 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:10:08 +0900 Subject: [PATCH 15/33] Add specific set_parameter method to HRI Engine(IF) --- goal_sender/scripts/route_guidance_engine.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index c5a92bb5..51231546 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -14,6 +14,7 @@ from __future__ import print_function import os import sys +import threading from pyrois import RoIS_HRI @@ -176,6 +177,14 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): 'Navigation': NavClient('http://localhost:8001') } + def set_parameter(self, component_ref, parameters): + status = None + if not component_ref in self.compoent_clients.keys(): + status = RoIS_HRI.ReturnCode_t.ERROR.value + else: + status = RoIS_HRI.ReturnCode_t.OK.value + return (status, "2") + class IF_server: """IF_Server -- GitLab From b2502651271b581a87fc1d71c8527fedb3fe469d Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:11:29 +0900 Subject: [PATCH 16/33] Add sample HRI Engine client --- .../scripts/route_guidance_engine_client.py | 136 ++++++++++++++++++ 1 file changed, 136 insertions(+) create mode 100644 goal_sender/scripts/route_guidance_engine_client.py 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 00000000..a1b00726 --- /dev/null +++ b/goal_sender/scripts/route_guidance_engine_client.py @@ -0,0 +1,136 @@ +# 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 +# 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): + (s, command_id) = self._proxy.set_parameter(component_ref, parameters) + status = RoIS_HRI.ReturnCode_t(s) + 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 -- GitLab From cdc21acf427d660e9cd0bea6bb2342b70d1eb710 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:14:48 +0900 Subject: [PATCH 17/33] Add test for HRI engine --- goal_sender/scripts/test_engine.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 goal_sender/scripts/test_engine.py diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py new file mode 100644 index 00000000..5b2c33cd --- /dev/null +++ b/goal_sender/scripts/test_engine.py @@ -0,0 +1,18 @@ +from route_guidance_engine_client import IF +import time + +s = 'http://127.0.0.1:8000' +# a = SystemIF(s) +# b = CommandIF(s) +# c = QueryIF(s) +# d = EventIF(s) +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') +print(i.set_parameter('Navigation',["hello","",20,"english","robotA"])) +#print(i.analysis_c_status()) +time.sleep(3) +i.disconnect() +print("finish") -- GitLab From cd6cb599438eea0a6c648e45b892455615c2762e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:20:54 +0900 Subject: [PATCH 18/33] Add component's set_parameter with HRIEngine --- goal_sender/scripts/route_guidance_engine.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index 51231546..1058bd44 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -179,10 +179,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): def set_parameter(self, component_ref, parameters): status = None - if not component_ref in self.compoent_clients.keys(): + if not component_ref in self.compoent_clients: status = RoIS_HRI.ReturnCode_t.ERROR.value else: - status = RoIS_HRI.ReturnCode_t.OK.value + target_component_client = self.compoent_clients[component_ref] + status = target_component_client.set_parameter("A", "B", "C") return (status, "2") -- GitLab From a55efeb8233cadffa75b813983cebac25b5aef55 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:42:14 +0900 Subject: [PATCH 19/33] Fix for calling set_parameter of Navigation component --- goal_sender/scripts/Navigation_client.py | 4 ++-- goal_sender/scripts/route_guidance_engine.py | 2 +- goal_sender/scripts/test_engine.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py index 3b27ea8d..7f0e97f5 100644 --- a/goal_sender/scripts/Navigation_client.py +++ b/goal_sender/scripts/Navigation_client.py @@ -41,8 +41,8 @@ class Command(RoIS_Common.Command): return status def set_parameter(self, target_position, time_limit, routing_policy): - s = self._proxy.set_parameter(target_position, time_limit, routing_policy) - status = RoIS_HRI.ReturnCode_t(s) + status = self._proxy.set_parameter(target_position, time_limit, routing_policy) + # status = RoIS_HRI.ReturnCode_t(status) return status diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index 1058bd44..a8c00537 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -183,7 +183,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): status = RoIS_HRI.ReturnCode_t.ERROR.value else: target_component_client = self.compoent_clients[component_ref] - status = target_component_client.set_parameter("A", "B", "C") + status = target_component_client.set_parameter(*parameters) return (status, "2") diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py index 5b2c33cd..cc48766b 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -11,7 +11,7 @@ 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') -print(i.set_parameter('Navigation',["hello","",20,"english","robotA"])) +print(i.set_parameter('Navigation',["hello","", "a"])) #print(i.analysis_c_status()) time.sleep(3) i.disconnect() -- GitLab From 7ab1e319ecbf9b9560f1db1a28ee90d263bf83c1 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 13:55:56 +0900 Subject: [PATCH 20/33] Upgrade for dynamic target position --- goal_sender/scripts/Navigation.py | 2 +- goal_sender/scripts/goal_sender_ros.py | 5 +++-- goal_sender/scripts/test_engine.py | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py index 976efeac..b1d10420 100755 --- a/goal_sender/scripts/Navigation.py +++ b/goal_sender/scripts/Navigation.py @@ -85,7 +85,7 @@ class Navigation(Event, Command, Query): self._component.Routing_Policy = "" def set_parameter(self, target_position, time_limit, routing_policy): - self._goal_sender.send_goal() + self._goal_sender.send_goal(target_position) status = RoIS_HRI.ReturnCode_t.OK.value return status diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py index 7958e1d9..c6058e48 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/goal_sender/scripts/goal_sender_ros.py @@ -42,8 +42,9 @@ class GoalSenderROS(object): 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)] + 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) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py index cc48766b..3ef202a8 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -11,7 +11,7 @@ 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') -print(i.set_parameter('Navigation',["hello","", "a"])) +print(i.set_parameter('Navigation',[[0, 5], "", "a"])) #print(i.analysis_c_status()) time.sleep(3) i.disconnect() -- GitLab From b320f7d1fa248fdda3c43b8b6f0e9d32cb1f54e3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 14:08:47 +0900 Subject: [PATCH 21/33] Add test of multiple destination --- goal_sender/scripts/test_engine.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py index 3ef202a8..7760dad5 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -1,18 +1,23 @@ +# 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 s = 'http://127.0.0.1:8000' -# a = SystemIF(s) -# b = CommandIF(s) -# c = QueryIF(s) -# d = EventIF(s) 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') -print(i.set_parameter('Navigation',[[0, 5], "", "a"])) +destinations = [[10, 0], [0, 20], [20, 20], [0, 0]] +for dest in destinations: + print(i.set_parameter('Navigation',[dest, "", ""])) + time.sleep(3) + #print(i.analysis_c_status()) -time.sleep(3) i.disconnect() print("finish") -- GitLab From 3841f7059abf18bca8aa49419b81361b91eac737 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 15:52:44 +0900 Subject: [PATCH 22/33] Check navigation status with "component_status" --- goal_sender/scripts/Navigation.py | 7 +++++++ goal_sender/scripts/Navigation_client.py | 9 +++++++-- goal_sender/scripts/route_guidance_engine.py | 7 +++++++ goal_sender/scripts/route_guidance_engine_client.py | 12 +++++++++--- goal_sender/scripts/test_engine.py | 7 +++++++ 5 files changed, 37 insertions(+), 5 deletions(-) diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py index b1d10420..d40706b9 100755 --- a/goal_sender/scripts/Navigation.py +++ b/goal_sender/scripts/Navigation.py @@ -89,6 +89,13 @@ class Navigation(Event, Command, Query): status = RoIS_HRI.ReturnCode_t.OK.value return status + def component_status(self): + print("In Navigation server") + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = self._goal_sender.get_goalsender_state().value + print("result: {}".format((status, c_status))) + return (status, c_status) + def event_dispatch(n): # n.person_detected(datetime.now().isoformat(), 1) diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py index 7f0e97f5..322980b3 100644 --- a/goal_sender/scripts/Navigation_client.py +++ b/goal_sender/scripts/Navigation_client.py @@ -42,6 +42,7 @@ class Command(RoIS_Common.Command): def set_parameter(self, target_position, time_limit, routing_policy): status = self._proxy.set_parameter(target_position, time_limit, routing_policy) + print("Navigation_Client: ", status) # status = RoIS_HRI.ReturnCode_t(status) return status @@ -54,8 +55,12 @@ class Query(RoIS_Common.Query): self._proxy = xmlrpc.client.ServerProxy(self._uri) def component_status(self): - (status, c_status) = self._proxy.component_status() - return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + print("In navigation client") + print(dir(self._proxy)) + status, c_status = self._proxy.component_status() + print("In navigation client(End)") + # return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + return (status, c_status) def get_parameter(self): (status, target_position, time_limit, routing_policy) = self._proxy.get_parameter() diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index a8c00537..0f28d63a 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -186,6 +186,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): status = target_component_client.set_parameter(*parameters) return (status, "2") + ### Remove ### + def get_navi_status(self): + print("In client get_navi_status") + result = self.compoent_clients['Navigation'].component_status() + print("AHIAHI: {}".format(result)) + return result + ############## class IF_server: """IF_Server diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py index a1b00726..4127b7f4 100644 --- a/goal_sender/scripts/route_guidance_engine_client.py +++ b/goal_sender/scripts/route_guidance_engine_client.py @@ -13,7 +13,7 @@ import time import xmlrpc.client # from pyrois import Service_Application_Base_example -from pyrois import RoIS_HRI +from pyrois import RoIS_HRI, RoIS_Common # import Service_Application_IF_test class SystemIF(RoIS_HRI.SystemIF): @@ -71,8 +71,8 @@ class CommandIF(RoIS_HRI.CommandIF): return (status, parameter_list) def set_parameter(self, component_ref, parameters): - (s, command_id) = self._proxy.set_parameter(component_ref, parameters) - status = RoIS_HRI.ReturnCode_t(s) + (status, command_id) = self._proxy.set_parameter(component_ref, parameters) + # status = RoIS_HRI.ReturnCode_t(s) return (status, command_id) def execute(self, command_unit_list): @@ -134,3 +134,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): 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_engine.py b/goal_sender/scripts/test_engine.py index 7760dad5..3ae9e1a4 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -6,6 +6,7 @@ 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) @@ -16,6 +17,12 @@ print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')) destinations = [[10, 0], [0, 20], [20, 20], [0, 0]] for dest in destinations: print(i.set_parameter('Navigation',[dest, "", ""])) + navi_status = Component_Status.BUSY + print(navi_status) + while navi_status != Component_Status.READY: + navi_status = i.get_navi_status() + print(navi_status) + time.sleep(1) time.sleep(3) #print(i.analysis_c_status()) -- GitLab From ff226a69046a6e74d20955a6b9e3b77a9da92683 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 15:57:37 +0900 Subject: [PATCH 23/33] Fix status upgrade process intest --- goal_sender/scripts/test_engine.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py index 3ae9e1a4..25d50637 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -18,10 +18,10 @@ destinations = [[10, 0], [0, 20], [20, 20], [0, 0]] for dest in destinations: print(i.set_parameter('Navigation',[dest, "", ""])) navi_status = Component_Status.BUSY - print(navi_status) + print("TEST: status = {}".format(navi_status)) while navi_status != Component_Status.READY: - navi_status = i.get_navi_status() - print(navi_status) + server_status, navi_status = i.get_navi_status() + print("TEST: status = {}".format(navi_status)) time.sleep(1) time.sleep(3) -- GitLab From 96fd916975179712f1a55f2f51c174cce54f5045 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 16:00:34 +0900 Subject: [PATCH 24/33] Remove code for debug --- goal_sender/scripts/Navigation.py | 2 -- goal_sender/scripts/Navigation_client.py | 4 ---- goal_sender/scripts/route_guidance_engine.py | 5 +---- 3 files changed, 1 insertion(+), 10 deletions(-) diff --git a/goal_sender/scripts/Navigation.py b/goal_sender/scripts/Navigation.py index d40706b9..879b2bdf 100755 --- a/goal_sender/scripts/Navigation.py +++ b/goal_sender/scripts/Navigation.py @@ -90,10 +90,8 @@ class Navigation(Event, Command, Query): return status def component_status(self): - print("In Navigation server") status = RoIS_HRI.ReturnCode_t.OK.value c_status = self._goal_sender.get_goalsender_state().value - print("result: {}".format((status, c_status))) return (status, c_status) diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py index 322980b3..87063ee9 100644 --- a/goal_sender/scripts/Navigation_client.py +++ b/goal_sender/scripts/Navigation_client.py @@ -42,7 +42,6 @@ class Command(RoIS_Common.Command): def set_parameter(self, target_position, time_limit, routing_policy): status = self._proxy.set_parameter(target_position, time_limit, routing_policy) - print("Navigation_Client: ", status) # status = RoIS_HRI.ReturnCode_t(status) return status @@ -55,10 +54,7 @@ class Query(RoIS_Common.Query): self._proxy = xmlrpc.client.ServerProxy(self._uri) def component_status(self): - print("In navigation client") - print(dir(self._proxy)) status, c_status = self._proxy.component_status() - print("In navigation client(End)") # return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) return (status, c_status) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index 0f28d63a..3629cd95 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -188,10 +188,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): ### Remove ### def get_navi_status(self): - print("In client get_navi_status") - result = self.compoent_clients['Navigation'].component_status() - print("AHIAHI: {}".format(result)) - return result + return self.compoent_clients['Navigation'].component_status() ############## class IF_server: -- GitLab From 34f06fbc4de4b89497cc9f59881390b715397c6e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 16:09:32 +0900 Subject: [PATCH 25/33] Fix status type for XMLRPC --- goal_sender/scripts/Navigation_client.py | 5 ++--- goal_sender/scripts/route_guidance_engine.py | 5 +++-- goal_sender/scripts/route_guidance_engine_client.py | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/goal_sender/scripts/Navigation_client.py b/goal_sender/scripts/Navigation_client.py index 87063ee9..fc616a48 100644 --- a/goal_sender/scripts/Navigation_client.py +++ b/goal_sender/scripts/Navigation_client.py @@ -42,7 +42,7 @@ class Command(RoIS_Common.Command): 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) + status = RoIS_HRI.ReturnCode_t(status) return status @@ -55,8 +55,7 @@ class Query(RoIS_Common.Query): def component_status(self): status, c_status = self._proxy.component_status() - # return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) - return (status, c_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() diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index 3629cd95..e3237a24 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -183,12 +183,13 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): status = RoIS_HRI.ReturnCode_t.ERROR.value else: target_component_client = self.compoent_clients[component_ref] - status = target_component_client.set_parameter(*parameters) + status = target_component_client.set_parameter(*parameters).value return (status, "2") ### Remove ### def get_navi_status(self): - return self.compoent_clients['Navigation'].component_status() + status, c_status = self.compoent_clients['Navigation'].component_status() + return (status.value, c_status.value) ############## class IF_server: diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py index 4127b7f4..73786572 100644 --- a/goal_sender/scripts/route_guidance_engine_client.py +++ b/goal_sender/scripts/route_guidance_engine_client.py @@ -72,7 +72,7 @@ class CommandIF(RoIS_HRI.CommandIF): def set_parameter(self, component_ref, parameters): (status, command_id) = self._proxy.set_parameter(component_ref, parameters) - # status = RoIS_HRI.ReturnCode_t(s) + status = RoIS_HRI.ReturnCode_t(status) return (status, command_id) def execute(self, command_unit_list): -- GitLab From 1760dae8cc0d5766909c96a9ee20dccaa6cf86ee Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 16:36:52 +0900 Subject: [PATCH 26/33] Remake list of goal position --- goal_sender/scripts/test_engine.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py index 25d50637..006b76c9 100644 --- a/goal_sender/scripts/test_engine.py +++ b/goal_sender/scripts/test_engine.py @@ -14,7 +14,7 @@ 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 = [[10, 0], [0, 20], [20, 20], [0, 0]] +destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] for dest in destinations: print(i.set_parameter('Navigation',[dest, "", ""])) navi_status = Component_Status.BUSY -- GitLab From 4f510e15e3cb2fa2ee8e8befadf43ab9ba65dfef Mon Sep 17 00:00:00 2001 From: tanacchi Date: Wed, 23 Oct 2019 16:44:53 +0900 Subject: [PATCH 27/33] Remove unnecessary files --- goal_sender/scripts/goal_sender_client.py | 104 ------------------- goal_sender/scripts/goal_sender_node.py | 116 ---------------------- goal_sender/scripts/goal_sender_ros.py | 56 ----------- 3 files changed, 276 deletions(-) delete mode 100644 goal_sender/scripts/goal_sender_client.py delete mode 100755 goal_sender/scripts/goal_sender_node.py delete mode 100644 goal_sender/scripts/goal_sender_ros.py diff --git a/goal_sender/scripts/goal_sender_client.py b/goal_sender/scripts/goal_sender_client.py deleted file mode 100644 index 239def84..00000000 --- a/goal_sender/scripts/goal_sender_client.py +++ /dev/null @@ -1,104 +0,0 @@ -# goal_sender_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 - -"""goal_sender_client -""" - -import threading -# import logging -import xmlrpc.client - -from pyrois import RoIS_Common, RoIS_HRI - - -class Command(RoIS_Common.Command): - """Command - """ - def __init__(self, uri): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - - def start(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.start()) - return status - - def stop(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) - return status - - def suspend(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) - return status - - def resume(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) - return status - - -class Query(RoIS_Common.Query): - """Query - """ - def __init__(self, uri): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - - def component_status(self): - (status, c_status) = self._proxy.component_status() - return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) - - -class Event(RoIS_Common.Event): - """Event - """ - def __init__(self, uri): - self._uri = uri - self._e_proxy = xmlrpc.client.ServerProxy(self._uri) - # 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 __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() diff --git a/goal_sender/scripts/goal_sender_node.py b/goal_sender/scripts/goal_sender_node.py deleted file mode 100755 index 07c545cf..00000000 --- a/goal_sender/scripts/goal_sender_node.py +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/env python - -# goal_sender_node.py -# -# Copyright 2018 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Component - -import sys -# import queue -import time -from datetime import datetime -import threading - -from pyrois import RoIS_Common, RoIS_HRI - -if sys.version_info.major == 2: - import 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): - pass - - -class Command(RoIS_Common.Command): - def __init__(self, c): - self._component = c - - def start(self): - print("Sending goal...") - self._component.send_goal() - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def stop(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def suspend(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def resume(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - -class Query(RoIS_Common.Query): - def __init__(self, c): - self._component = c - - def component_status(self): - status = RoIS_HRI.ReturnCode_t.OK.value - c_status = RoIS_Common.Component_Status.UNINITIALIZED.value - return (status, c_status) - - -class Event(RoIS_Common.Event): - def __init__(self, c): - self._component = c - self.event_queue = queue.Queue() - - 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) - - -class GoalSender(Event, Command, Query): - pass - - -def event_dispatch(gs): - # gs.person_detected(datetime.now().isoformat(), 1) - time.sleep(0.1) - # gs.person_detected(datetime.now().isoformat(), 1) - - -import rospy -from goal_sender_ros import GoalSenderROS - -def example_gs(port): - print("Starting node...") - component = GoalSenderROS() - gs = GoalSender(component) - print("Goalsender constructed") - - # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(gs,)) - t.start() - - # start the XML-RPC server - server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(gs) - server.register_introspection_functions() - server.register_multicall_functions() - server.serve_forever() - - -if __name__ == '__main__': - rospy.init_node('goal_sender_node') - example_gs(8000) diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py deleted file mode 100644 index c6058e48..00000000 --- a/goal_sender/scripts/goal_sender_ros.py +++ /dev/null @@ -1,56 +0,0 @@ -#/usr/bin/env python - -import rospy -import actionlib - -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal -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 - -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): - # Table for conversion from GoalStatus to Component_Status - # http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html - GoalSenderState = { - g_state.PENDING: c_state.READY, - g_state.ACTIVE: c_state.BUSY, - g_state.PREEMPTED: c_state.READY, - g_state.SUCCEEDED: c_state.READY, - g_state.ABORTED: c_state.ERROR, - g_state.PREEMPTING: c_state.BUSY, - g_state.RECALLING: c_state.BUSY, - g_state.RECALLED: c_state.READY, - g_state.LOST: c_state.ERROR - } - - def __init__(self): - self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) - 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 get_goalsender_state(self): - state = self.action_client.get_state() - return GoalSenderROS.GoalSenderState[state] - -# If you remove this comment out, it does not work. -# rospy.init_node('goal_sender') -- GitLab From 3d1c4f51660c2f8cf73b4a42e7b04c7f38cd8c3f Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 24 Oct 2019 16:28:35 +0900 Subject: [PATCH 28/33] Readd goal_sender_ros.py --- goal_sender/scripts/goal_sender_ros.py | 56 ++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 goal_sender/scripts/goal_sender_ros.py diff --git a/goal_sender/scripts/goal_sender_ros.py b/goal_sender/scripts/goal_sender_ros.py new file mode 100644 index 00000000..c6058e48 --- /dev/null +++ b/goal_sender/scripts/goal_sender_ros.py @@ -0,0 +1,56 @@ +#/usr/bin/env python + +import rospy +import actionlib + +from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal +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 + +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): + # Table for conversion from GoalStatus to Component_Status + # http://docs.ros.org/kinetic/api/actionlib_msgs/html/msg/GoalStatus.html + GoalSenderState = { + g_state.PENDING: c_state.READY, + g_state.ACTIVE: c_state.BUSY, + g_state.PREEMPTED: c_state.READY, + g_state.SUCCEEDED: c_state.READY, + g_state.ABORTED: c_state.ERROR, + g_state.PREEMPTING: c_state.BUSY, + g_state.RECALLING: c_state.BUSY, + g_state.RECALLED: c_state.READY, + g_state.LOST: c_state.ERROR + } + + def __init__(self): + self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + 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 get_goalsender_state(self): + state = self.action_client.get_state() + return GoalSenderROS.GoalSenderState[state] + +# If you remove this comment out, it does not work. +# rospy.init_node('goal_sender') -- GitLab From 2998207871c1ac8abc9f1148ca475144a251b73b Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 24 Oct 2019 17:17:22 +0900 Subject: [PATCH 29/33] Ignore virtualenv directory --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 63967614..fff456dd 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,5 @@ CMakeLists.txt __pycache__ *.pyc + +*venv -- GitLab From a55301bf8dd24f06d2e973d730798a44ca2c1134 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 24 Oct 2019 17:59:28 +0900 Subject: [PATCH 30/33] Make unique command_id --- goal_sender/scripts/route_guidance_engine.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index e3237a24..ada84f45 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -173,6 +173,7 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): """ def __init__(self, Engine): super().__init__(Engine) + self.command_id = 0 self.compoent_clients = { 'Navigation': NavClient('http://localhost:8001') } @@ -184,7 +185,8 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF): else: target_component_client = self.compoent_clients[component_ref] status = target_component_client.set_parameter(*parameters).value - return (status, "2") + self.command_id += 1 + return (status, str(self.command_id)) ### Remove ### def get_navi_status(self): -- GitLab From edef961dc5fe327a142fbd904a7e782371097d3e Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 24 Oct 2019 18:07:40 +0900 Subject: [PATCH 31/33] Use RoIS_Service.Service_Application_Base on Engine --- goal_sender/scripts/route_guidance_engine.py | 4 ++-- goal_sender/scripts/route_guidance_engine_client.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index ada84f45..bb5d1485 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -16,7 +16,7 @@ import os import sys import threading -from pyrois import RoIS_HRI +from pyrois import RoIS_HRI, RoIS_Service if sys.version_info.major == 2: import SocketServer @@ -168,7 +168,7 @@ class EventIF(RoIS_HRI.EventIF): from Navigation_client import Navigation_Client as NavClient -class IF(SystemIF, CommandIF, QueryIF, EventIF): +class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_HRI.Service_Application_Base): """IF """ def __init__(self, Engine): diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py index 73786572..ce707709 100644 --- a/goal_sender/scripts/route_guidance_engine_client.py +++ b/goal_sender/scripts/route_guidance_engine_client.py @@ -13,7 +13,7 @@ import time import xmlrpc.client # from pyrois import Service_Application_Base_example -from pyrois import RoIS_HRI, RoIS_Common +from pyrois import RoIS_HRI, RoIS_Common, RoIS_Service # import Service_Application_IF_test class SystemIF(RoIS_HRI.SystemIF): @@ -121,7 +121,7 @@ class EventIF(RoIS_HRI.EventIF): # class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF_test.Service_Application_IF): -class IF(SystemIF, CommandIF, QueryIF, EventIF): +class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF """ def __init__(self, uri): -- GitLab From 595ed95fd718eb3354714c86cc731853e8b7c125 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 24 Oct 2019 18:07:40 +0900 Subject: [PATCH 32/33] Use RoIS_Service.Service_Application_Base on Engine --- goal_sender/scripts/route_guidance_engine.py | 2 +- goal_sender/scripts/route_guidance_engine_client.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/goal_sender/scripts/route_guidance_engine.py b/goal_sender/scripts/route_guidance_engine.py index bb5d1485..335976b1 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/goal_sender/scripts/route_guidance_engine.py @@ -168,7 +168,7 @@ class EventIF(RoIS_HRI.EventIF): from Navigation_client import Navigation_Client as NavClient -class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_HRI.Service_Application_Base): +class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF """ def __init__(self, Engine): diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/goal_sender/scripts/route_guidance_engine_client.py index ce707709..6abe2d97 100644 --- a/goal_sender/scripts/route_guidance_engine_client.py +++ b/goal_sender/scripts/route_guidance_engine_client.py @@ -120,7 +120,6 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) -# class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF_test.Service_Application_IF): class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF """ -- GitLab From 2779a3e7ca7a6844377ef350335e9db0ea800057 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Thu, 24 Oct 2019 18:30:41 +0900 Subject: [PATCH 33/33] Remove pyrois directory --- goal_sender/scripts/pyrois/.gitignore | 1 - .../scripts/pyrois/HRI_Engine_client.py | 129 ---------- .../scripts/pyrois/HRI_Engine_example.py | 205 ---------------- .../scripts/pyrois/Person_Detection.py | 134 ---------- .../scripts/pyrois/Person_Detection_client.py | 104 -------- goal_sender/scripts/pyrois/RoIS_Common.py | 67 ----- goal_sender/scripts/pyrois/RoIS_HRI.py | 142 ----------- goal_sender/scripts/pyrois/RoIS_Service.py | 55 ----- .../Service_Application_Base_example.py | 100 -------- .../scripts/pyrois/Service_Application_IF.py | 81 ------ goal_sender/scripts/pyrois/__init__.py | 0 goal_sender/scripts/pyrois/unittest.py | 231 ------------------ 12 files changed, 1249 deletions(-) delete mode 100644 goal_sender/scripts/pyrois/.gitignore delete mode 100644 goal_sender/scripts/pyrois/HRI_Engine_client.py delete mode 100644 goal_sender/scripts/pyrois/HRI_Engine_example.py delete mode 100644 goal_sender/scripts/pyrois/Person_Detection.py delete mode 100644 goal_sender/scripts/pyrois/Person_Detection_client.py delete mode 100644 goal_sender/scripts/pyrois/RoIS_Common.py delete mode 100644 goal_sender/scripts/pyrois/RoIS_HRI.py delete mode 100644 goal_sender/scripts/pyrois/RoIS_Service.py delete mode 100644 goal_sender/scripts/pyrois/Service_Application_Base_example.py delete mode 100644 goal_sender/scripts/pyrois/Service_Application_IF.py delete mode 100644 goal_sender/scripts/pyrois/__init__.py delete mode 100644 goal_sender/scripts/pyrois/unittest.py diff --git a/goal_sender/scripts/pyrois/.gitignore b/goal_sender/scripts/pyrois/.gitignore deleted file mode 100644 index bee8a64b..00000000 --- a/goal_sender/scripts/pyrois/.gitignore +++ /dev/null @@ -1 +0,0 @@ -__pycache__ diff --git a/goal_sender/scripts/pyrois/HRI_Engine_client.py b/goal_sender/scripts/pyrois/HRI_Engine_client.py deleted file mode 100644 index 6c79f528..00000000 --- a/goal_sender/scripts/pyrois/HRI_Engine_client.py +++ /dev/null @@ -1,129 +0,0 @@ -# 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 xmlrpc.client - -from pyrois import RoIS_HRI - - -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): - (s, command_id) = self._proxy.set_parameter(component_ref, parameters) - status = RoIS_HRI.ReturnCode_t(s) - 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): - """IF - """ - def __init__(self, uri): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - -if __name__ == "__main__": - pass diff --git a/goal_sender/scripts/pyrois/HRI_Engine_example.py b/goal_sender/scripts/pyrois/HRI_Engine_example.py deleted file mode 100644 index 7b6770a6..00000000 --- a/goal_sender/scripts/pyrois/HRI_Engine_example.py +++ /dev/null @@ -1,205 +0,0 @@ -# HRI_Engine_example.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 - -"""HRI_Engine_example -""" - -from __future__ import print_function - -import os -import sys - -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) - - -class IF(SystemIF, CommandIF, QueryIF, EventIF): - """IF - """ - pass - - -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 MyHRIE: - """IF_Server - """ - def __init__(self): - self.state = False - - -def test_engine(port): - """test_engine - """ - IF_server(port).run(IF(MyHRIE())) - - -if __name__ == "__main__": - pass diff --git a/goal_sender/scripts/pyrois/Person_Detection.py b/goal_sender/scripts/pyrois/Person_Detection.py deleted file mode 100644 index 790033af..00000000 --- a/goal_sender/scripts/pyrois/Person_Detection.py +++ /dev/null @@ -1,134 +0,0 @@ -# Person_Detection.py -# -# Copyright 2018 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Component - -"""Person_Detection -""" - -import sys -# import queue -import time -from datetime import datetime -import threading - -from pyrois import RoIS_Common, RoIS_HRI - -if sys.version_info.major == 2: - import 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 Command(RoIS_Common.Command): - """Command - """ - def __init__(self, c): - self._component = c - - def start(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def stop(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def suspend(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def resume(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - -class Query(RoIS_Common.Query): - """Query - """ - def __init__(self, c): - self._component = c - - def component_status(self): - status = RoIS_HRI.ReturnCode_t.OK.value - c_status = RoIS_Common.Component_Status.UNINITIALIZED.value - return (status, c_status) - - -class Event(RoIS_Common.Event): - """Event - """ - def __init__(self, c): - self._component = c - self.event_queue = queue.Queue() - - def poll_event(self): - """poll_event - """ - msg = self.event_queue.get() - return msg - - def person_detected(self, timestamp, number): - """person_detected - """ - msg = xmlrpc.client.dumps((timestamp, number), 'person_detected') - self.event_queue.put(msg) - - -class Person_Detection(Event, Command, Query): - """Person_Detection - """ - pass - - -class component: - """component - """ - def __init__(self): - self._state = False - - -def event_dispatch(pd): - """event_dispatch - """ - pd.person_detected(datetime.now().isoformat(), 1) - time.sleep(0.1) - pd.person_detected(datetime.now().isoformat(), 1) - - -def example_pd(port): - """example_pd - """ - c = component() - pd = Person_Detection(c) - - # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(pd,)) - t.start() - - # start the XML-RPC server - server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(pd) - server.register_introspection_functions() - server.register_multicall_functions() - server.serve_forever() - - -if __name__ == '__main__': - example_pd(8000) diff --git a/goal_sender/scripts/pyrois/Person_Detection_client.py b/goal_sender/scripts/pyrois/Person_Detection_client.py deleted file mode 100644 index 77f4af4b..00000000 --- a/goal_sender/scripts/pyrois/Person_Detection_client.py +++ /dev/null @@ -1,104 +0,0 @@ -# Person_Detection_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 - -"""Person_Detection_Client -""" - -import threading -# import logging -import xmlrpc.client - -from pyrois import RoIS_Common, RoIS_HRI - - -class Command(RoIS_Common.Command): - """Command - """ - def __init__(self, uri): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - - def start(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.start()) - return status - - def stop(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.stop()) - return status - - def suspend(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.suspend()) - return status - - def resume(self): - status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) - return status - - -class Query(RoIS_Common.Query): - """Query - """ - def __init__(self, uri): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - - def component_status(self): - (status, c_status) = self._proxy.component_status() - return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) - - -class Event(RoIS_Common.Event): - """Event - """ - def __init__(self, uri): - self._uri = uri - self._e_proxy = xmlrpc.client.ServerProxy(self._uri) - # 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 Person_Detection_Client(Command, Query, Event): - """Person_Detection_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() diff --git a/goal_sender/scripts/pyrois/RoIS_Common.py b/goal_sender/scripts/pyrois/RoIS_Common.py deleted file mode 100644 index 7cacd5aa..00000000 --- a/goal_sender/scripts/pyrois/RoIS_Common.py +++ /dev/null @@ -1,67 +0,0 @@ -# RoIS_Common.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Component -""" -RoIS_Common -""" -import abc -import enum - -class Component_Status(enum.Enum): - """Component_Status - """ - UNINITIALIZED = 0 - READY = 1 - BUSY = 2 - WARNING = 3 - ERROR = 4 - -class Command: - """Command - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def start(self): - """ - start () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def stop(self): - """ - stop () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def suspend(self): - """ - suspend () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def resume(self): - """ - resume () -> status: RoIS_HRI.ReturnCode_t - """ - pass - -class Query: - """Quary - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def component_status(self): - """ - component_status () -> (status: RoIS_HRI.ReturnCode_t, status: Component_Status) - """ - pass - -class Event: - """Event - """ - pass diff --git a/goal_sender/scripts/pyrois/RoIS_HRI.py b/goal_sender/scripts/pyrois/RoIS_HRI.py deleted file mode 100644 index 4f3b7536..00000000 --- a/goal_sender/scripts/pyrois/RoIS_HRI.py +++ /dev/null @@ -1,142 +0,0 @@ -# RoIS_HRI.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 - -"""RoIS_HRI -""" -import abc -import enum - -class ReturnCode_t(enum.Enum): - """ReturnCode_t - """ - OK = 1 - ERROR = 2 - BAD_PARAMETER = 3 - UNSUPPORTED = 4 - OUT_OF_RESOURCES = 5 - TIMEOUT = 6 - -class SystemIF: - """ - class SystemIF(object) - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def connect(self): - """ - connect() -> ReturnCode_t: status - """ - pass - @abc.abstractmethod - def disconnect(self): - """ - disconnect() -> ReturnCode_t: status - """ - pass - @abc.abstractmethod - def get_profile(self, condition): - """ - get_profile(condition) -> (status, profile) - """ - pass - @abc.abstractmethod - def get_error_detail(self, error_id, condition): - """ - get_error_detail(error_id,condition) -> (status,results) - """ - pass - -class CommandIF: - """ - class CommandIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def search(self, condition): - """ - search(condition) -> (status, component_ref_list) - """ - pass - @abc.abstractmethod - def bind(self, component_ref): - """ - bind(component_ref) -> status - """ - pass - @abc.abstractmethod - def bind_any(self, condition): - """ - bind_any(condition) -> (status,component_ref_list) - """ - pass - @abc.abstractmethod - def release(self, component_ref): - """ - release(component_ref) -> status - """ - pass - @abc.abstractmethod - def get_parameter(self, component_ref): - """ - get_parameter(self,component_ref) -> (status,parameter_list) - """ - pass - @abc.abstractmethod - def set_parameter(self, component_ref, parameters): - """ - set_parameter(self, component_ref, parameters) -> (status,command_id) - """ - pass - @abc.abstractmethod - def execute(self, command_unit_list): - """ - execute(command_unit_list) -> status - """ - pass - @abc.abstractmethod - def get_command_result(self, command_id, condition): - """ - get_command_result(self, command_id, condition) -> (status,results) - """ - pass - -class QueryIF: - """ - class QueryIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def query(self, query_type, condition): - """ - query(self, query_type, condition) -> (status,results) - """ - pass - -class EventIF: - """ - class EventIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def subscribe(self, event_type, condition): - """ - subscribe(self, event_type, condition) -> (status,subscribe_id) - """ - pass - @abc.abstractmethod - def unsubscribe(self, subscribe_id): - """ - unsubscribe(self,subscribe_id) -> status - """ - pass - @abc.abstractmethod - def get_event_detail(self, event_id, condition): - """ - get_event_detail(self,event_id,condition) -> (status,results) - """ - pass diff --git a/goal_sender/scripts/pyrois/RoIS_Service.py b/goal_sender/scripts/pyrois/RoIS_Service.py deleted file mode 100644 index 7cd57e5c..00000000 --- a/goal_sender/scripts/pyrois/RoIS_Service.py +++ /dev/null @@ -1,55 +0,0 @@ -# RoIS_Service.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 - -"""RoIS_Service -""" - -import abc -import enum - -class Completed_Status(enum.Enum): - """Completed_Status - """ - OK = 1 - ERROR = 2 - ABORT = 3 - OUT_OF_RESOURCES = 4 - TIMEOUT = 5 - -class ErrorType(enum.Enum): - """ErrorType - """ - ENGINE_INTERNAL_ERROR = 1 - COMPONENT_INTERNEL_ERROR = 2 - COMPONENT_NOT_RESPONDING = 3 - USER_DEFINE_ERROR = 4 - -class Service_Application_Base: - """ - class ServiceApplicationBase(object) - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def completed(self, command_id, status): - """ - connect(command_id, status) : void - """ - pass - @abc.abstractmethod - def notify_error(self, error_id, error_type): - """ - notify_error(error_id, error_type) : void - """ - pass - @abc.abstractmethod - def notify_event(self, event_id, event_type, subscribe_id, expire): - """ - notify_event(self, event_id, event_type, subscribe_id, expire) : void - """ - pass diff --git a/goal_sender/scripts/pyrois/Service_Application_Base_example.py b/goal_sender/scripts/pyrois/Service_Application_Base_example.py deleted file mode 100644 index 0d350047..00000000 --- a/goal_sender/scripts/pyrois/Service_Application_Base_example.py +++ /dev/null @@ -1,100 +0,0 @@ -# Service_Application_Base_example.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 - -"""Service_Application_Base_example -""" - -import sys -import queue -import time -import threading - -from pyrois import RoIS_Service - -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 Service_Application_Base(RoIS_Service.Service_Application_Base): - """Service_Application_Base - """ - def __init__(self): - self.event_queue = queue.Queue() - - def poll_event(self): - """poll_event - """ - msg = self.event_queue.get() - return msg - - def completed(self, command_id, status): - msg = xmlrpc.client.dumps((command_id, status), 'completed') - self.event_queue.put(msg) - - def notify_error(self, error_id, error_type): - msg = xmlrpc.client.dumps((error_id, error_type), 'notify_error') - self.event_queue.put(msg) - - def notify_event(self, event_id, event_type, subscribe_id, expire): - msg = xmlrpc.client.dumps( - (event_id, event_type, subscribe_id, expire), 'notify_event') - self.event_queue.put(msg) - - -def event_dispatch(sa): - """event_dispatch - """ - sa.completed("0", RoIS_Service.Completed_Status.OK.value) - time.sleep(0.1) - sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) - time.sleep(0.2) - sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") - - -# def event_dispatch_long(sa): -# sa.completed("0", RoIS_Service.Completed_Status.OK.value) -# time.sleep(1) -# sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) -# time.sleep(60) -# sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") - -def example_sa(port): - """example_sa - """ - sa = Service_Application_Base() - - # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(sa,)) - t.start() - - # start the XML-RPC server - server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(sa) - server.register_introspection_functions() - server.register_multicall_functions() - # print("server running") - server.serve_forever() - - -if __name__ == '__main__': - example_sa(8000) diff --git a/goal_sender/scripts/pyrois/Service_Application_IF.py b/goal_sender/scripts/pyrois/Service_Application_IF.py deleted file mode 100644 index c1ca652d..00000000 --- a/goal_sender/scripts/pyrois/Service_Application_IF.py +++ /dev/null @@ -1,81 +0,0 @@ -# Service_Application_IF.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 - -"""Service_Application_IF -""" - -import xmlrpc.client -import logging -import logging.handlers - -from pyrois import RoIS_Service - - -class Service_Application_IF(RoIS_Service.Service_Application_Base): - """Service_Application_IF - """ - def __init__(self, uri, logger=None): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - if logger is not None: - self.logger = logger - while True: - self.poll_event() - - def poll_event(self): - """poll_event - """ - # print("poll") - msg = self._proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - # print(params,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.logger.debug('received completed event' - + command_id - + RoIS_Service.Completed_Status(status).name) - - def notify_error(self, error_id, error_type): - self.logger.debug('received error event' - + error_id - + RoIS_Service.ErrorType(error_type).name) - - def notify_event(self, event_id, event_type, subscribe_id, expire): - self.logger.debug('received event' - + event_id - + event_type - + subscribe_id - + expire) - - -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__': - pass diff --git a/goal_sender/scripts/pyrois/__init__.py b/goal_sender/scripts/pyrois/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/goal_sender/scripts/pyrois/unittest.py b/goal_sender/scripts/pyrois/unittest.py deleted file mode 100644 index b6dfeaec..00000000 --- a/goal_sender/scripts/pyrois/unittest.py +++ /dev/null @@ -1,231 +0,0 @@ -# unittest.py -# -# Copyright 2018 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 - -"""unittest -""" - -import time -from multiprocessing import Process, Queue -import unittest -import http -import xmlrpc.client -import sysconfig - -from pyrois import RoIS_HRI, RoIS_Common -from pyrois import Service_Application_IF, Service_Application_Base_example -from pyrois import HRI_Engine_client, HRI_Engine_example -from pyrois import Person_Detection_client, Person_Detection - - -class TestServericeApplicationIF(unittest.TestCase): - """TestServericeApplicationIF - """ - def setUp(self): - # start the server process - self.ps = Process( - target=Service_Application_Base_example.example_sa, args=(8000,)) - self.ps.start() - time.sleep(0.1) - - def test_IF(self): - """ test_IF - """ - - q = Queue() - # start the client process - pc = Process(target=Service_Application_IF.example_sa_IF, - args=("http://127.0.0.1:8000/", q)) - pc.start() - - time.sleep(1) - - num_events = 0 - while not q.empty(): - msg = q.get() - num_events += 1 - # print(msg.asctime,msg.name,msg.levelname,msg.message) - - if pc.is_alive(): - pc.terminate() - - return self.assertEqual(num_events, 3) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - while self.ps.is_alive(): - time.sleep(0.1) - - -class TestHRIEngineIF(unittest.TestCase): - """TestHRIEngineIF - """ - - def setUp(self): - # start the server process - self.ps = Process(target=HRI_Engine_example.test_engine, args=(8000,)) - self.ps.start() - time.sleep(0.1) - - def test_IF(self): - """test_IF - """ - - with xmlrpc.client.ServerProxy("http://127.0.0.1:8000/") as proxy: - a = HRI_Engine_client.SystemIF(proxy) - b = HRI_Engine_client.CommandIF(proxy) - - try: - res = [ - a.connect(), - a.connect(), - b.bind(''), - a.disconnect(), - a.disconnect(), - b.bind('')] - # print(res) - except xmlrpc.client.ProtocolError as err: - print(err) - - return self.assertEqual(res, - [ - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - RoIS_HRI.ReturnCode_t.OK]) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - time.sleep(0.1) - - -class TestHRIEngineIF_integrated(unittest.TestCase): - """TestHRIEngineIF_integrated - """ - def setUp(self): - # start the server process - self.ps = Process(target=HRI_Engine_example.test_engine, args=(8000,)) - self.ps.start() - time.sleep(0.1) - - def test_IF(self): - """test_IF - """ - - try: - a = HRI_Engine_client.IF("http://127.0.0.1:8000/") - res = [ - a.connect(), - a.connect(), - a.get_profile(""), - a.get_error_detail("0", ""), - - a.search(""), - a.bind(""), - a.bind_any(""), - a.release(""), - a.get_parameter(""), - a.set_parameter("", ""), - a.execute(""), - a.get_command_result("", ""), - - a.query("", ""), - - a.subscribe("", ""), - a.unsubscribe(""), - a.get_event_detail("", ""), - - a.disconnect(), - a.disconnect(), - a.bind('')] - # print(res) - except xmlrpc.client.ProtocolError as err: - print(err) - - return self.assertEqual(res, - [RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - (RoIS_HRI.ReturnCode_t.OK, "Unsupported"), - (RoIS_HRI.ReturnCode_t.OK, "None"), - - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - (RoIS_HRI.ReturnCode_t.OK, "0"), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - - (RoIS_HRI.ReturnCode_t.OK, "0"), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - RoIS_HRI.ReturnCode_t.OK]) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - time.sleep(0.1) - - -class TestPD(unittest.TestCase): - """TestPD - """ - - def setUp(self): - # start the server process - self.ps = Process(target=Person_Detection.example_pd, args=(8000,)) - self.ps.start() - time.sleep(0.5) - - def test_IF(self): - """test_IF - """ - a = Person_Detection_client.Person_Detection_Client("http://127.0.0.1:8000/") - try: - res = [ - a.start(), - a.stop(), - a.suspend(), - a.resume(), - a.component_status(), - len(a.events)] - except xmlrpc.client.ProtocolError as err: - print(err) - - return self.assertEqual(res, - [ - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, RoIS_Common.Component_Status.UNINITIALIZED), - 2]) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - time.sleep(0.1) - - -if __name__ == '__main__': - print("python", sysconfig.get_python_version()) - print("platform:", sysconfig.get_platform()) - unittest.main() -- GitLab