From 7f48e6a8855c6a27594476c069c0ee6985f30c89 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Fri, 11 Oct 2019 15:03:49 +0900 Subject: [PATCH 01/13] Add sample publisher and subscriber --- connection_test/CMakeLists.txt | 205 ++++++++++++++++++++++++++++ connection_test/package.xml | 65 +++++++++ connection_test/scripts/listener.py | 60 ++++++++ connection_test/scripts/talker.py | 56 ++++++++ 4 files changed, 386 insertions(+) create mode 100644 connection_test/CMakeLists.txt create mode 100644 connection_test/package.xml create mode 100755 connection_test/scripts/listener.py create mode 100755 connection_test/scripts/talker.py diff --git a/connection_test/CMakeLists.txt b/connection_test/CMakeLists.txt new file mode 100644 index 00000000..923cdcd7 --- /dev/null +++ b/connection_test/CMakeLists.txt @@ -0,0 +1,205 @@ +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 +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## 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( +# INCLUDE_DIRS include +# LIBRARIES connection_test +# CATKIN_DEPENDS rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/connection_test.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## 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/connection_test_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## 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 00000000..50b7ff92 --- /dev/null +++ b/connection_test/package.xml @@ -0,0 +1,65 @@ + + + connection_test + 0.0.0 + The connection_test package + + + + + tanacchi + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + rospy + std_msgs + rospy + std_msgs + rospy + std_msgs + + + + + + + + diff --git a/connection_test/scripts/listener.py b/connection_test/scripts/listener.py new file mode 100755 index 00000000..ed27e1e5 --- /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/talker.py b/connection_test/scripts/talker.py new file mode 100755 index 00000000..033dc449 --- /dev/null +++ b/connection_test/scripts/talker.py @@ -0,0 +1,56 @@ +#!/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 published std_msgs/Strings messages +## to the 'chatter' topic + +import rospy +from std_msgs.msg import String + +def talker(): + pub = rospy.Publisher('chatter', String, queue_size=10) + rospy.init_node('talker', anonymous=True) + rate = rospy.Rate(10) # 10hz + while not rospy.is_shutdown(): + hello_str = "hello world %s" % rospy.get_time() + rospy.loginfo(hello_str) + pub.publish(hello_str) + rate.sleep() + +if __name__ == '__main__': + try: + talker() + except rospy.ROSInterruptException: + pass -- GitLab From e1b6936bdc6e74771e5c9e6789b1073bd99cdbc9 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 14 Oct 2019 18:37:16 +0900 Subject: [PATCH 02/13] Add pyrois package --- .../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 ++++++++ connection_test/scripts/pyrois/RoIS_Common.py | 67 +++++ .../scripts/pyrois/RoIS_Common.pyc | Bin 0 -> 2779 bytes connection_test/scripts/pyrois/RoIS_HRI.py | 142 +++++++++++ connection_test/scripts/pyrois/RoIS_HRI.pyc | Bin 0 -> 6257 bytes .../scripts/pyrois/RoIS_Service.py | 55 +++++ .../Service_Application_Base_example.py | 100 ++++++++ .../scripts/pyrois/Service_Application_IF.py | 81 ++++++ connection_test/scripts/pyrois/__init__.py | 0 connection_test/scripts/pyrois/__init__.pyc | Bin 0 -> 164 bytes connection_test/scripts/pyrois/unittest.py | 231 ++++++++++++++++++ 14 files changed, 1248 insertions(+) create mode 100644 connection_test/scripts/pyrois/HRI_Engine_client.py create mode 100644 connection_test/scripts/pyrois/HRI_Engine_example.py create mode 100644 connection_test/scripts/pyrois/Person_Detection.py create mode 100644 connection_test/scripts/pyrois/Person_Detection_client.py create mode 100644 connection_test/scripts/pyrois/RoIS_Common.py create mode 100644 connection_test/scripts/pyrois/RoIS_Common.pyc create mode 100644 connection_test/scripts/pyrois/RoIS_HRI.py create mode 100644 connection_test/scripts/pyrois/RoIS_HRI.pyc create mode 100644 connection_test/scripts/pyrois/RoIS_Service.py create mode 100644 connection_test/scripts/pyrois/Service_Application_Base_example.py create mode 100644 connection_test/scripts/pyrois/Service_Application_IF.py create mode 100644 connection_test/scripts/pyrois/__init__.py create mode 100644 connection_test/scripts/pyrois/__init__.pyc create mode 100644 connection_test/scripts/pyrois/unittest.py 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 00000000..6c79f528 --- /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 00000000..7b6770a6 --- /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 00000000..790033af --- /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 00000000..77f4af4b --- /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 00000000..7cacd5aa --- /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 GIT binary patch literal 2779 zcmd5;TTc@~6h6D9g_hz4G|@y8;?t5S`{aX&1PYBd(JE7kp^4dScLrIcyUos2QeX9_ z_ybM+0eZe!y40E|F@dnnoSr#zJJ)Zf^kbp+edF8vh)%x}|8H{BhKVHVGIdD0bnH-< znVfg2>yj*ya_Oi<2P~YSuS8#n_PaAA%OvMXRtgdKXGzvc&K05x$tuYOlC?s_Q(WqJ z*Kqm*lbT3donSld_tT^n|KzW2Zm^ssgEUcz2|6Y;LtW_Z@L&V0A2Ly_JL|=iHD%pP z*d5B1sLT4yP?uFGQ+Jj`$E#q~Z6?!u*OvKR6Je6R)ZvJn)lM>r@LHl?VYDw{6UT6ZyQ32j%orCUzo1s7;ue zWp8Eq(FSS!+PS}RfQ^n;#|IZ20vm9v={*<#E2#W0 zGuL=hp|m#X;D4KqPN&^!1A`2s1jqO*8>kn8M1bt?s<4|ixN5FpA(m_J*BG zD1lo_V4=7PvIugE$)zO65+C%Kn??cPnyfncMg56iBt=@F1R9uyq@7F)jy=X~?3-9( zJ@27Lrp&w)Yw+gTggyXfSTl8M?tiNtyl!tol??lqX))u}sm_ilCR{?hy}ZAfr4 literal 0 HcmV?d00001 diff --git a/connection_test/scripts/pyrois/RoIS_HRI.py b/connection_test/scripts/pyrois/RoIS_HRI.py new file mode 100644 index 00000000..4f3b7536 --- /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 GIT binary patch literal 6257 zcmc&&ZEqVz5MJAf5a9`_8>x z(*}u8MMC@!B>o-}KLF<0y*-~@J9cRf9NlDZ_jY%lnP+F_uJYG<{m)y!fA6#OSE2u3 z(ItN+5ir&vSz*01s7ScmUY#zbK7|h3aygh@lM;l$9L-C-}Kv4Yv>bd zm_*xgAY7HfDAB-e%=(msAQ+?~Nm80%RIn=atgsHLw!}K5%`)qd0#(+jG2V7oFc;hA zSFbnd7h>tK3;|n(OAyNtRT2)gR5cQ=8+m=y_#wz>YD$H(i zzR%Sq_gbGX+&ynlb6AGGeMily9QQ?2d6DP)$6@nnocti0kG!8mH+G-OrcC^%A4ie!RTxLE5>hp# zpM(P?n}d@i4rSBY{gc57#|G!=k`UE$wKV$Ez}qKMiT=*r%ojkZec@6g?+a03kIDGU z3=MKYN&B4~v|@Y8wx?!$Dz<0U_88{TtU4SATgnCr?JU3NN!hR(IdS(ffxK>p;75CH zV-`F~Idved%9t$fhC`Iq6NOx%8r|P|r z{h&Ns_AN5}pMvfNb!R#Uq5M|>ImlTkIGml4cGu|6;sZ=vGw^*$Lm9qDLb-z^J_>un zA)rABo@-Y+#(SHMbl6Y>9#8#^bHp3^p?uV>U>H9QKra!35aPn4m^2y9meWFVqZ(bTC66LzDVaTR1QihatFj z64X=>Uf0)$V(a$yE&!orQYD_RsQ)+)xK1R9zWf3NBL4`SZWRzo z!S1)|lGwdksg_ntm#X;FWZRDWeJ={81=2t0`9cF}8jzQ5usvskt;y*WY}ahhihX9? z_LwA3&zwg<#<`JKNoZo4fSf_I5&5PWgd=gyp!c?21qW0LFY%9Ooo7Gp58_Bf%1y+P z+Y4nH^`{-};;D?v!07uqT7XC)dJbA}Jv+?ZFbW834wOE-(~}PtHYXO09-B2H_1W`T z=Q}96MY}Et^3Ehk*NaYOICTuj|L)ToX%4VZ`0K?1qaMw?nuwn8B$CogI+i1B6zWTq zsi}G>As2M#ULKv!VO~O2uo8)bLWnZYBv6}y2ll~vjx8KH&diwV?Hqn27+fi_s#5G6HrPgY*y`i^a0Ba#K7L z{!nSwYm~z%RL1M5uIGg`q-JZMY-XXraTHRW@djfd&!NI0J(^Jge6mWLI%~v-)YF^9 zjcKLn{IU#Xgk3xgiqB@3XulT$Q9WW!G*4bw+{wD1Lj!Nj3uf$9}Mim)I@L(vClWFxBO7sQmF{+4gWF}ux)1@cs zp*a-0M+;>XdjjQBR_V5@P6p!DST$Owcvj)XUGMh<<}qQ#sZo2P#-B>059qn#S#phb z=|MZxbA~0F7p)(u(458#WX=V7kyhNNe()gf=uZj_T2}krRMYA9FUOq+b`4!i54-r_ z(-kRy1OT1NA8BWtnxk}j8ZA~liS)~cRqbbjXoEHQglk6LzL~eiFv`#wdt|o&yGV}0 z`&@p~h?-{~yYv^roXXF!w>dukocOVyM~#SXT$O$<&;^_6dDU+Qnoj>kYRbMD=p7A^ z-$n*;kSZ66CvX`52x61O$bGtX_CDkv(k{>Fl2CAze}YUS$?*g zcl|n3_-WyJaO=v^h0s)UbE+KS4|? Date: Mon, 14 Oct 2019 18:42:06 +0900 Subject: [PATCH 03/13] Make Talker class --- connection_test/scripts/talker.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/connection_test/scripts/talker.py b/connection_test/scripts/talker.py index 033dc449..d08f0a02 100755 --- a/connection_test/scripts/talker.py +++ b/connection_test/scripts/talker.py @@ -39,18 +39,22 @@ import rospy from std_msgs.msg import String -def talker(): - pub = rospy.Publisher('chatter', String, queue_size=10) - rospy.init_node('talker', anonymous=True) - rate = rospy.Rate(10) # 10hz - while not rospy.is_shutdown(): +class Talker(object): + def __init__(self): + self.pub = rospy.Publisher('chatter', String, queue_size=10) + rospy.init_node('talker', anonymous=True) + self.rate = rospy.Rate(10) # 10hz + + def publish(self): hello_str = "hello world %s" % rospy.get_time() rospy.loginfo(hello_str) - pub.publish(hello_str) - rate.sleep() + self.pub.publish(hello_str) + self.rate.sleep() if __name__ == '__main__': + talker = Talker() try: - talker() + while not rospy.is_shutdown(): + talker.publish() except rospy.ROSInterruptException: pass -- GitLab From 55cd3b9cb7ddf10a525f42295925e5f0d9b5dca1 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 14 Oct 2019 21:24:18 +0900 Subject: [PATCH 04/13] Ignore __pycache__ directory in pyrois --- connection_test/scripts/pyrois/.gitignore | 1 + 1 file changed, 1 insertion(+) create mode 100644 connection_test/scripts/pyrois/.gitignore diff --git a/connection_test/scripts/pyrois/.gitignore b/connection_test/scripts/pyrois/.gitignore new file mode 100644 index 00000000..bee8a64b --- /dev/null +++ b/connection_test/scripts/pyrois/.gitignore @@ -0,0 +1 @@ +__pycache__ -- GitLab From 0e50929c14fd80e5d5739462045b18784c1857ae Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 14 Oct 2019 21:45:23 +0900 Subject: [PATCH 05/13] Add rosnode with pyrois --- connection_test/scripts/Person_Detection.py | 145 +++++++++++++ .../scripts/Person_Detection_client.py | 107 ++++++++++ connection_test/scripts/talker.py | 197 +++++++++++++----- connection_test/scripts/talker_old.py | 60 ++++++ 4 files changed, 454 insertions(+), 55 deletions(-) create mode 100755 connection_test/scripts/Person_Detection.py create mode 100755 connection_test/scripts/Person_Detection_client.py create mode 100755 connection_test/scripts/talker_old.py diff --git a/connection_test/scripts/Person_Detection.py b/connection_test/scripts/Person_Detection.py new file mode 100755 index 00000000..cc86dcc5 --- /dev/null +++ b/connection_test/scripts/Person_Detection.py @@ -0,0 +1,145 @@ +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 + 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 + + +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() + +# import rospy +# from std_msgs.msg import String + +# class Talker(object): + # def __init__(self): + # self.pub = rospy.Publisher('chatter', String, queue_size=10) + # rospy.init_node('talker', anonymous=True) + # self.rate = rospy.Rate(10) # 10hz + + # def publish(self): + # hello_str = "hello world %s" % rospy.get_time() + # rospy.loginfo(hello_str) + # self.pub.publish(hello_str) + # self.rate.sleep() + + +if __name__ == '__main__': + example_pd(8000) + # # talker = Talker() + # # try: + # # while not rospy.is_shutdown(): + # # talker.publish() + # # except rospy.ROSInterruptException: + # # pass diff --git a/connection_test/scripts/Person_Detection_client.py b/connection_test/scripts/Person_Detection_client.py new file mode 100755 index 00000000..f8846e3b --- /dev/null +++ b/connection_test/scripts/Person_Detection_client.py @@ -0,0 +1,107 @@ +# 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 + +# import xmlrpc.client as xmlrpc_client +import xmlrpclib 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/talker.py b/connection_test/scripts/talker.py index d08f0a02..af17b1f6 100755 --- a/connection_test/scripts/talker.py +++ b/connection_test/scripts/talker.py @@ -1,60 +1,147 @@ #!/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 published std_msgs/Strings messages -## to the 'chatter' topic - -import rospy -from std_msgs.msg import String - -class Talker(object): + +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 + 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 + + +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.pub = rospy.Publisher('chatter', String, queue_size=10) - rospy.init_node('talker', anonymous=True) - self.rate = rospy.Rate(10) # 10hz + 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() + +# import rospy +# from std_msgs.msg import String + +# class Talker(object): + # def __init__(self): + # self.pub = rospy.Publisher('chatter', String, queue_size=10) + # rospy.init_node('talker', anonymous=True) + # self.rate = rospy.Rate(10) # 10hz + + # def publish(self): + # hello_str = "hello world %s" % rospy.get_time() + # rospy.loginfo(hello_str) + # self.pub.publish(hello_str) + # self.rate.sleep() - def publish(self): - hello_str = "hello world %s" % rospy.get_time() - rospy.loginfo(hello_str) - self.pub.publish(hello_str) - self.rate.sleep() if __name__ == '__main__': - talker = Talker() - try: - while not rospy.is_shutdown(): - talker.publish() - except rospy.ROSInterruptException: - pass + example_pd(8000) + # # talker = Talker() + # # try: + # # while not rospy.is_shutdown(): + # # talker.publish() + # # except rospy.ROSInterruptException: + # # pass diff --git a/connection_test/scripts/talker_old.py b/connection_test/scripts/talker_old.py new file mode 100755 index 00000000..d08f0a02 --- /dev/null +++ b/connection_test/scripts/talker_old.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 published std_msgs/Strings messages +## to the 'chatter' topic + +import rospy +from std_msgs.msg import String + +class Talker(object): + def __init__(self): + self.pub = rospy.Publisher('chatter', String, queue_size=10) + rospy.init_node('talker', anonymous=True) + self.rate = rospy.Rate(10) # 10hz + + def publish(self): + hello_str = "hello world %s" % rospy.get_time() + rospy.loginfo(hello_str) + self.pub.publish(hello_str) + self.rate.sleep() + +if __name__ == '__main__': + talker = Talker() + try: + while not rospy.is_shutdown(): + talker.publish() + except rospy.ROSInterruptException: + pass -- GitLab From 266efe66d912723b05ac1225bbf53fa0ff490dd2 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 14 Oct 2019 23:32:44 +0900 Subject: [PATCH 06/13] Ignore .pyc file and __pycache__ directory --- connection_test/scripts/.gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 connection_test/scripts/.gitignore diff --git a/connection_test/scripts/.gitignore b/connection_test/scripts/.gitignore new file mode 100644 index 00000000..8d35cb32 --- /dev/null +++ b/connection_test/scripts/.gitignore @@ -0,0 +1,2 @@ +__pycache__ +*.pyc -- GitLab From beea6578cce690fe4eb2707141344e2de1cd07cb Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 14 Oct 2019 23:34:16 +0900 Subject: [PATCH 07/13] Fix Person_Detection_client for python2 --- .../scripts/Person_Detection_client.py | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/connection_test/scripts/Person_Detection_client.py b/connection_test/scripts/Person_Detection_client.py index f8846e3b..5fc9af96 100755 --- a/connection_test/scripts/Person_Detection_client.py +++ b/connection_test/scripts/Person_Detection_client.py @@ -14,8 +14,10 @@ import sys import threading # import logging -# import xmlrpc.client as xmlrpc_client -import xmlrpclib as xmlrpc_client +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 @@ -25,7 +27,7 @@ class Command(RoIS_Common.Command): """ def __init__(self, uri): self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) + self._proxy = xmlrpc_client.ServerProxy(self._uri) def start(self): status = RoIS_HRI.ReturnCode_t(self._proxy.start()) @@ -49,7 +51,7 @@ class Query(RoIS_Common.Query): """ def __init__(self, uri): self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) + self._proxy = xmlrpc_client.ServerProxy(self._uri) def component_status(self): (status, c_status) = self._proxy.component_status() @@ -61,7 +63,7 @@ class Event(RoIS_Common.Event): """ def __init__(self, uri): self._uri = uri - self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self._e_proxy = xmlrpc_client.ServerProxy(self._uri) # if logger is not None: # self.logger = logger @@ -82,7 +84,7 @@ class Event(RoIS_Common.Event): """poll_event """ msg = self._e_proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) + (params, methodname) = xmlrpc_client.loads(msg) #self.logger.debug('poll_event: '+methodname) # print(params,methodname) if methodname == 'person_detected': @@ -101,7 +103,7 @@ class Person_Detection_Client(Command, Query, Event): """ def __init__(self, uri): self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self._proxy = xmlrpc_client.ServerProxy(self._uri) + self._e_proxy = xmlrpc_client.ServerProxy(self._uri) self.events = [] self.start_th() -- GitLab From 517710128b77eeb8cb71f2d6a9fe71f1ab577ce3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 14 Oct 2019 23:36:54 +0900 Subject: [PATCH 08/13] Include Talker into Person_Detection_Client class --- connection_test/scripts/talker.py | 57 ++++++++++++++++++------------- 1 file changed, 33 insertions(+), 24 deletions(-) diff --git a/connection_test/scripts/talker.py b/connection_test/scripts/talker.py index af17b1f6..15bd8607 100755 --- a/connection_test/scripts/talker.py +++ b/connection_test/scripts/talker.py @@ -1,7 +1,6 @@ #!/usr/bin/env python import sys -# import queue import time from datetime import datetime import threading @@ -35,6 +34,7 @@ class Command(RoIS_Common.Command): self._component = c def start(self): + print("START!!!") status = RoIS_HRI.ReturnCode_t.OK.value return status @@ -82,12 +82,38 @@ class Event(RoIS_Common.Event): msg = xmlrpc_client.dumps((timestamp, number), 'person_detected') self.event_queue.put(msg) +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 Person_Detection(Event, Command, Query): """Person_Detection """ - pass + def __init__(self, c): + super(Person_Detection, self).__init__(c) + self.talker = None + + def start(self): + self.talker = Talker() + status = RoIS_HRI.ReturnCode_t.OK.value + return status + def stop(self): + if self.talker != None: + self.talker.publish() + status = RoIS_HRI.ReturnCode_t.OK.value + return status class component: """component @@ -121,27 +147,10 @@ def example_pd(port): server.register_multicall_functions() server.serve_forever() -# import rospy -# from std_msgs.msg import String - -# class Talker(object): - # def __init__(self): - # self.pub = rospy.Publisher('chatter', String, queue_size=10) - # rospy.init_node('talker', anonymous=True) - # self.rate = rospy.Rate(10) # 10hz - - # def publish(self): - # hello_str = "hello world %s" % rospy.get_time() - # rospy.loginfo(hello_str) - # self.pub.publish(hello_str) - # self.rate.sleep() - if __name__ == '__main__': - example_pd(8000) - # # talker = Talker() - # # try: - # # while not rospy.is_shutdown(): - # # talker.publish() - # # except rospy.ROSInterruptException: - # # pass + try: + rospy.init_node('talker', anonymous=True) + example_pd(8000) + except rospy.ROSInterruptException: + pass -- GitLab From a088b0742e76cd9877a5b920bc9d3b0289bc65a3 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Mon, 14 Oct 2019 23:45:55 +0900 Subject: [PATCH 09/13] Add test for lanching ros-node with rois 1. roscore 2. rosrun connection_test talker.py 3. python test.py --- connection_test/scripts/test.py | 11 +++++++++++ 1 file changed, 11 insertions(+) create mode 100644 connection_test/scripts/test.py diff --git a/connection_test/scripts/test.py b/connection_test/scripts/test.py new file mode 100644 index 00000000..603fe246 --- /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()) -- GitLab From 432daf83b9f43112ffef4d68640b4cbec66acd8c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 16:21:22 +0900 Subject: [PATCH 10/13] Include Talker instance as component MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit この使い方が合っているのかは不明 --- connection_test/scripts/talker.py | 56 +++++++++++-------------------- 1 file changed, 19 insertions(+), 37 deletions(-) diff --git a/connection_test/scripts/talker.py b/connection_test/scripts/talker.py index 15bd8607..8fed8244 100755 --- a/connection_test/scripts/talker.py +++ b/connection_test/scripts/talker.py @@ -26,6 +26,20 @@ else: """ 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 @@ -34,11 +48,12 @@ class Command(RoIS_Common.Command): self._component = c def start(self): - print("START!!!") + 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 @@ -82,44 +97,11 @@ class Event(RoIS_Common.Event): msg = xmlrpc_client.dumps((timestamp, number), 'person_detected') self.event_queue.put(msg) -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 Person_Detection(Event, Command, Query): """Person_Detection """ - def __init__(self, c): - super(Person_Detection, self).__init__(c) - self.talker = None - - def start(self): - self.talker = Talker() - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def stop(self): - if self.talker != None: - self.talker.publish() - status = RoIS_HRI.ReturnCode_t.OK.value - return status - -class component: - """component - """ - def __init__(self): - self._state = False + pass def event_dispatch(pd): @@ -133,8 +115,8 @@ def event_dispatch(pd): def example_pd(port): """example_pd """ - c = component() - pd = Person_Detection(c) + talker = Talker() + pd = Person_Detection(talker) # start the timer to dispatch events t = threading.Timer(0.1, event_dispatch, args=(pd,)) -- GitLab From dcc52e7290908da028ab5bc815e3c3f56c31254c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 17:00:09 +0900 Subject: [PATCH 11/13] Organize auto-generated files --- connection_test/CMakeLists.txt | 155 --------------------------------- connection_test/package.xml | 50 +---------- 2 files changed, 3 insertions(+), 202 deletions(-) diff --git a/connection_test/CMakeLists.txt b/connection_test/CMakeLists.txt index 923cdcd7..a2eaf3cc 100644 --- a/connection_test/CMakeLists.txt +++ b/connection_test/CMakeLists.txt @@ -12,86 +12,6 @@ find_package(catkin REQUIRED COMPONENTS std_msgs ) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - ################################### ## catkin specific configuration ## ################################### @@ -102,10 +22,7 @@ find_package(catkin REQUIRED COMPONENTS ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES connection_test # CATKIN_DEPENDS rospy std_msgs -# DEPENDS system_lib ) ########### @@ -119,78 +36,6 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/connection_test.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## 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/connection_test_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - ############# ## Testing ## ############# diff --git a/connection_test/package.xml b/connection_test/package.xml index 50b7ff92..21c8e6fa 100644 --- a/connection_test/package.xml +++ b/connection_test/package.xml @@ -4,50 +4,10 @@ 0.0.0 The connection_test package - - - - tanacchi + tanacchi + MIT - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin rospy std_msgs @@ -57,9 +17,5 @@ std_msgs - - - - - + -- GitLab From 1f1846812d5883aa60faaa1106016b092df6cd75 Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 17:01:29 +0900 Subject: [PATCH 12/13] Remove unnecessary files --- connection_test/scripts/Person_Detection.py | 145 -------------------- connection_test/scripts/talker_old.py | 60 -------- 2 files changed, 205 deletions(-) delete mode 100755 connection_test/scripts/Person_Detection.py delete mode 100755 connection_test/scripts/talker_old.py diff --git a/connection_test/scripts/Person_Detection.py b/connection_test/scripts/Person_Detection.py deleted file mode 100755 index cc86dcc5..00000000 --- a/connection_test/scripts/Person_Detection.py +++ /dev/null @@ -1,145 +0,0 @@ -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 - 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 - - -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() - -# import rospy -# from std_msgs.msg import String - -# class Talker(object): - # def __init__(self): - # self.pub = rospy.Publisher('chatter', String, queue_size=10) - # rospy.init_node('talker', anonymous=True) - # self.rate = rospy.Rate(10) # 10hz - - # def publish(self): - # hello_str = "hello world %s" % rospy.get_time() - # rospy.loginfo(hello_str) - # self.pub.publish(hello_str) - # self.rate.sleep() - - -if __name__ == '__main__': - example_pd(8000) - # # talker = Talker() - # # try: - # # while not rospy.is_shutdown(): - # # talker.publish() - # # except rospy.ROSInterruptException: - # # pass diff --git a/connection_test/scripts/talker_old.py b/connection_test/scripts/talker_old.py deleted file mode 100755 index d08f0a02..00000000 --- a/connection_test/scripts/talker_old.py +++ /dev/null @@ -1,60 +0,0 @@ -#!/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 published std_msgs/Strings messages -## to the 'chatter' topic - -import rospy -from std_msgs.msg import String - -class Talker(object): - def __init__(self): - self.pub = rospy.Publisher('chatter', String, queue_size=10) - rospy.init_node('talker', anonymous=True) - self.rate = rospy.Rate(10) # 10hz - - def publish(self): - hello_str = "hello world %s" % rospy.get_time() - rospy.loginfo(hello_str) - self.pub.publish(hello_str) - self.rate.sleep() - -if __name__ == '__main__': - talker = Talker() - try: - while not rospy.is_shutdown(): - talker.publish() - except rospy.ROSInterruptException: - pass -- GitLab From 9635f99ae8f6899adc39c923939865f11924373c Mon Sep 17 00:00:00 2001 From: tanacchi Date: Tue, 15 Oct 2019 17:14:03 +0900 Subject: [PATCH 13/13] Add README on connection_test package --- connection_test/scripts/README.md | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 connection_test/scripts/README.md diff --git a/connection_test/scripts/README.md b/connection_test/scripts/README.md new file mode 100644 index 00000000..6e78e79c --- /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 でキルできなかったらターミナルごと閉じること。 -- GitLab