diff --git a/connection_test/CMakeLists.txt b/connection_test/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..a2eaf3cc1308cc452817a1488df415f2399e6133 --- /dev/null +++ b/connection_test/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 2.8.3) +project(connection_test) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# CATKIN_DEPENDS rospy std_msgs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_connection_test.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/connection_test/package.xml b/connection_test/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..21c8e6fa2146ae6b223e300996b8f05428d9a190 --- /dev/null +++ b/connection_test/package.xml @@ -0,0 +1,21 @@ + + + connection_test + 0.0.0 + The connection_test package + + tanacchi + + MIT + + catkin + rospy + std_msgs + rospy + std_msgs + rospy + std_msgs + + + + diff --git a/connection_test/scripts/.gitignore b/connection_test/scripts/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..8d35cb3277ff6eb0d637147c09e95934604a431c --- /dev/null +++ b/connection_test/scripts/.gitignore @@ -0,0 +1,2 @@ +__pycache__ +*.pyc diff --git a/connection_test/scripts/Person_Detection_client.py b/connection_test/scripts/Person_Detection_client.py new file mode 100755 index 0000000000000000000000000000000000000000..5fc9af96b10ab2bb0278127a6ce1ef8813291b62 --- /dev/null +++ b/connection_test/scripts/Person_Detection_client.py @@ -0,0 +1,109 @@ +# 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 sys +import threading +# import logging + +if sys.version_info.major == 2: + import xmlrpclib as xmlrpc_client +else: + import xmlrpc.client as 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/connection_test/scripts/README.md b/connection_test/scripts/README.md new file mode 100644 index 0000000000000000000000000000000000000000..6e78e79c2261bcaa2e786e763953e45e578616c5 --- /dev/null +++ b/connection_test/scripts/README.md @@ -0,0 +1,14 @@ +# connection_test + +RoISとROSの接続テスト + +ROSノードをコンポーネントとする +エンジンを作成し、クライアントからのリクエストでトピックを送信する。 + +# How to use +1. roscore を起動 +2. 別ターミナルで`rosrun connection_test talker.py`を実行 +3. さらに別のターミナルで `python test.py` を実行 + +Already Used 的なエラーが出たらポートの番号を変えてみること。 +SIGINT でキルできなかったらターミナルごと閉じること。 diff --git a/connection_test/scripts/listener.py b/connection_test/scripts/listener.py new file mode 100755 index 0000000000000000000000000000000000000000..ed27e1e5fdae3fd5a1d6a63e809bed7a91bb5330 --- /dev/null +++ b/connection_test/scripts/listener.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Revision $Id$ + +## Simple talker demo that listens to std_msgs/Strings published +## to the 'chatter' topic + +import rospy +from std_msgs.msg import String + +def callback(data): + rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.data) + +def listener(): + + # In ROS, nodes are uniquely named. If two nodes with the same + # name are launched, the previous one is kicked off. The + # anonymous=True flag means that rospy will choose a unique + # name for our 'listener' node so that multiple listeners can + # run simultaneously. + rospy.init_node('listener', anonymous=True) + + rospy.Subscriber('chatter', String, callback) + + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + +if __name__ == '__main__': + listener() diff --git a/connection_test/scripts/pyrois/.gitignore b/connection_test/scripts/pyrois/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..bee8a64b79a99590d5303307144172cfe824fbf7 --- /dev/null +++ b/connection_test/scripts/pyrois/.gitignore @@ -0,0 +1 @@ +__pycache__ diff --git a/connection_test/scripts/pyrois/HRI_Engine_client.py b/connection_test/scripts/pyrois/HRI_Engine_client.py new file mode 100644 index 0000000000000000000000000000000000000000..6c79f528e6ea5a60beb45ab6be5d76b62c9da60e --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/HRI_Engine_example.py b/connection_test/scripts/pyrois/HRI_Engine_example.py new file mode 100644 index 0000000000000000000000000000000000000000..7b6770a684a29adc44d368e15088d536b8b4f74f --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/Person_Detection.py b/connection_test/scripts/pyrois/Person_Detection.py new file mode 100644 index 0000000000000000000000000000000000000000..790033afff729bd87a083547d0443eb52cf2f78b --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/Person_Detection_client.py b/connection_test/scripts/pyrois/Person_Detection_client.py new file mode 100644 index 0000000000000000000000000000000000000000..77f4af4b204237003d69ddddd830e188dfaf133a --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/RoIS_Common.py b/connection_test/scripts/pyrois/RoIS_Common.py new file mode 100644 index 0000000000000000000000000000000000000000..7cacd5aa76b0a6ace2b768aa39506d159302583d --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/RoIS_Common.pyc b/connection_test/scripts/pyrois/RoIS_Common.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4c139e1a0bb898757ba6e3007e3b8d2c07dac4ef Binary files /dev/null and b/connection_test/scripts/pyrois/RoIS_Common.pyc differ diff --git a/connection_test/scripts/pyrois/RoIS_HRI.py b/connection_test/scripts/pyrois/RoIS_HRI.py new file mode 100644 index 0000000000000000000000000000000000000000..4f3b7536f35efa251be9072c8dc5f09eb20f5c80 --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/RoIS_HRI.pyc b/connection_test/scripts/pyrois/RoIS_HRI.pyc new file mode 100644 index 0000000000000000000000000000000000000000..59dd27fdc82a83272b2284ff63f5763aa479cebc Binary files /dev/null and b/connection_test/scripts/pyrois/RoIS_HRI.pyc differ diff --git a/connection_test/scripts/pyrois/RoIS_Service.py b/connection_test/scripts/pyrois/RoIS_Service.py new file mode 100644 index 0000000000000000000000000000000000000000..7cd57e5cbd2de7d75b03bc4bc4383d018bce2c11 --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/Service_Application_Base_example.py b/connection_test/scripts/pyrois/Service_Application_Base_example.py new file mode 100644 index 0000000000000000000000000000000000000000..0d35004779147ab2de592a71f94ca595d78a57e6 --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/Service_Application_IF.py b/connection_test/scripts/pyrois/Service_Application_IF.py new file mode 100644 index 0000000000000000000000000000000000000000..c1ca652d6aa90e49b10fccfc8cac43aa08fc0cd5 --- /dev/null +++ b/connection_test/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/connection_test/scripts/pyrois/__init__.py b/connection_test/scripts/pyrois/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/connection_test/scripts/pyrois/__init__.pyc b/connection_test/scripts/pyrois/__init__.pyc new file mode 100644 index 0000000000000000000000000000000000000000..fbe0aff4f0b3d3761fc8169bbcdc88515888c72d Binary files /dev/null and b/connection_test/scripts/pyrois/__init__.pyc differ diff --git a/connection_test/scripts/pyrois/unittest.py b/connection_test/scripts/pyrois/unittest.py new file mode 100644 index 0000000000000000000000000000000000000000..b6dfeaec5ebc440c39868966b7b285940f827c0b --- /dev/null +++ b/connection_test/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() diff --git a/connection_test/scripts/talker.py b/connection_test/scripts/talker.py new file mode 100755 index 0000000000000000000000000000000000000000..8fed82446dcd4708e32f70a21b1aee3f39335993 --- /dev/null +++ b/connection_test/scripts/talker.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python + +import sys +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import Queue as queue + import SocketServer + import SimpleXMLRPCServer + import xmlrpclib as xmlrpc_client + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import queue + import socketserver + import xmlrpc.server + import xmlrpc.client as xmlrpc_client + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + +import rospy +from std_msgs.msg import String + +class Talker(object): + def __init__(self): + self.pub = rospy.Publisher('chatter', String, queue_size=10) + rospy.loginfo("Talker init") + print("Talker init") + + def publish(self): + hello_str = "hello world %s" % rospy.get_time() + rospy.loginfo(hello_str) + self.pub.publish(hello_str) + + +class Command(RoIS_Common.Command): + """Command + """ + def __init__(self, c): + self._component = c + + def start(self): + self._component.publish() + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + self._component = None + 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 + + +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 + """ + talker = Talker() + pd = Person_Detection(talker) + + # 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__': + try: + rospy.init_node('talker', anonymous=True) + example_pd(8000) + except rospy.ROSInterruptException: + pass diff --git a/connection_test/scripts/test.py b/connection_test/scripts/test.py new file mode 100644 index 0000000000000000000000000000000000000000..603fe246f1e69231775853dad087f870cbe70ded --- /dev/null +++ b/connection_test/scripts/test.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +import Person_Detection_client +import time + +port = "http://localhost:8000" +client = Person_Detection_client.Person_Detection_Client(port) + +print(client.start()) +time.sleep(5) +print(client.stop())