diff --git a/connection_test/CMakeLists.txt b/connection_test/CMakeLists.txt deleted file mode 100644 index a2eaf3cc1308cc452817a1488df415f2399e6133..0000000000000000000000000000000000000000 --- a/connection_test/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -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 deleted file mode 100644 index 21c8e6fa2146ae6b223e300996b8f05428d9a190..0000000000000000000000000000000000000000 --- a/connection_test/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - 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 deleted file mode 100644 index 8d35cb3277ff6eb0d637147c09e95934604a431c..0000000000000000000000000000000000000000 --- a/connection_test/scripts/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -__pycache__ -*.pyc diff --git a/connection_test/scripts/Person_Detection_client.py b/connection_test/scripts/Person_Detection_client.py deleted file mode 100755 index 5fc9af96b10ab2bb0278127a6ce1ef8813291b62..0000000000000000000000000000000000000000 --- a/connection_test/scripts/Person_Detection_client.py +++ /dev/null @@ -1,109 +0,0 @@ -# Person_Detection_Client.py -# -# Copyright 2018 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Engine - -"""Person_Detection_Client -""" - -import 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 deleted file mode 100644 index 6e78e79c2261bcaa2e786e763953e45e578616c5..0000000000000000000000000000000000000000 --- a/connection_test/scripts/README.md +++ /dev/null @@ -1,14 +0,0 @@ -# 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 deleted file mode 100755 index ed27e1e5fdae3fd5a1d6a63e809bed7a91bb5330..0000000000000000000000000000000000000000 --- a/connection_test/scripts/listener.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 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 deleted file mode 100644 index bee8a64b79a99590d5303307144172cfe824fbf7..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/.gitignore +++ /dev/null @@ -1 +0,0 @@ -__pycache__ diff --git a/connection_test/scripts/pyrois/Person_Detection.py b/connection_test/scripts/pyrois/Person_Detection.py deleted file mode 100644 index 790033afff729bd87a083547d0443eb52cf2f78b..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/Person_Detection.py +++ /dev/null @@ -1,134 +0,0 @@ -# Person_Detection.py -# -# Copyright 2018 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Component - -"""Person_Detection -""" - -import sys -# import queue -import time -from datetime import datetime -import threading - -from pyrois import RoIS_Common, RoIS_HRI - -if sys.version_info.major == 2: - import Queue as queue - import SocketServer - import SimpleXMLRPCServer - - class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): - pass -else: - import queue - import socketserver - import xmlrpc.server - - class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): - """ThreadingXMLRPCServer - """ - pass - - -class Command(RoIS_Common.Command): - """Command - """ - def __init__(self, c): - self._component = c - - def start(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def stop(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def suspend(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - def resume(self): - status = RoIS_HRI.ReturnCode_t.OK.value - return status - - -class Query(RoIS_Common.Query): - """Query - """ - def __init__(self, c): - self._component = c - - def component_status(self): - status = RoIS_HRI.ReturnCode_t.OK.value - c_status = RoIS_Common.Component_Status.UNINITIALIZED.value - return (status, c_status) - - -class Event(RoIS_Common.Event): - """Event - """ - def __init__(self, c): - self._component = c - self.event_queue = queue.Queue() - - def poll_event(self): - """poll_event - """ - msg = self.event_queue.get() - return msg - - def person_detected(self, timestamp, number): - """person_detected - """ - msg = xmlrpc.client.dumps((timestamp, number), 'person_detected') - self.event_queue.put(msg) - - -class Person_Detection(Event, Command, Query): - """Person_Detection - """ - pass - - -class component: - """component - """ - def __init__(self): - self._state = False - - -def event_dispatch(pd): - """event_dispatch - """ - pd.person_detected(datetime.now().isoformat(), 1) - time.sleep(0.1) - pd.person_detected(datetime.now().isoformat(), 1) - - -def example_pd(port): - """example_pd - """ - c = component() - pd = Person_Detection(c) - - # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(pd,)) - t.start() - - # start the XML-RPC server - server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(pd) - server.register_introspection_functions() - server.register_multicall_functions() - server.serve_forever() - - -if __name__ == '__main__': - example_pd(8000) diff --git a/connection_test/scripts/pyrois/RoIS_Common.py b/connection_test/scripts/pyrois/RoIS_Common.py deleted file mode 100644 index 7cacd5aa76b0a6ace2b768aa39506d159302583d..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/RoIS_Common.py +++ /dev/null @@ -1,67 +0,0 @@ -# RoIS_Common.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# For python3 -# For HRI Component -""" -RoIS_Common -""" -import abc -import enum - -class Component_Status(enum.Enum): - """Component_Status - """ - UNINITIALIZED = 0 - READY = 1 - BUSY = 2 - WARNING = 3 - ERROR = 4 - -class Command: - """Command - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def start(self): - """ - start () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def stop(self): - """ - stop () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def suspend(self): - """ - suspend () -> status: RoIS_HRI.ReturnCode_t - """ - pass - @abc.abstractmethod - def resume(self): - """ - resume () -> status: RoIS_HRI.ReturnCode_t - """ - pass - -class Query: - """Quary - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def component_status(self): - """ - component_status () -> (status: RoIS_HRI.ReturnCode_t, status: Component_Status) - """ - pass - -class Event: - """Event - """ - pass diff --git a/connection_test/scripts/pyrois/RoIS_Common.pyc b/connection_test/scripts/pyrois/RoIS_Common.pyc deleted file mode 100644 index 4c139e1a0bb898757ba6e3007e3b8d2c07dac4ef..0000000000000000000000000000000000000000 Binary files a/connection_test/scripts/pyrois/RoIS_Common.pyc and /dev/null differ diff --git a/connection_test/scripts/pyrois/RoIS_HRI.py b/connection_test/scripts/pyrois/RoIS_HRI.py deleted file mode 100644 index 4f3b7536f35efa251be9072c8dc5f09eb20f5c80..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/RoIS_HRI.py +++ /dev/null @@ -1,142 +0,0 @@ -# RoIS_HRI.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 - -"""RoIS_HRI -""" -import abc -import enum - -class ReturnCode_t(enum.Enum): - """ReturnCode_t - """ - OK = 1 - ERROR = 2 - BAD_PARAMETER = 3 - UNSUPPORTED = 4 - OUT_OF_RESOURCES = 5 - TIMEOUT = 6 - -class SystemIF: - """ - class SystemIF(object) - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def connect(self): - """ - connect() -> ReturnCode_t: status - """ - pass - @abc.abstractmethod - def disconnect(self): - """ - disconnect() -> ReturnCode_t: status - """ - pass - @abc.abstractmethod - def get_profile(self, condition): - """ - get_profile(condition) -> (status, profile) - """ - pass - @abc.abstractmethod - def get_error_detail(self, error_id, condition): - """ - get_error_detail(error_id,condition) -> (status,results) - """ - pass - -class CommandIF: - """ - class CommandIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def search(self, condition): - """ - search(condition) -> (status, component_ref_list) - """ - pass - @abc.abstractmethod - def bind(self, component_ref): - """ - bind(component_ref) -> status - """ - pass - @abc.abstractmethod - def bind_any(self, condition): - """ - bind_any(condition) -> (status,component_ref_list) - """ - pass - @abc.abstractmethod - def release(self, component_ref): - """ - release(component_ref) -> status - """ - pass - @abc.abstractmethod - def get_parameter(self, component_ref): - """ - get_parameter(self,component_ref) -> (status,parameter_list) - """ - pass - @abc.abstractmethod - def set_parameter(self, component_ref, parameters): - """ - set_parameter(self, component_ref, parameters) -> (status,command_id) - """ - pass - @abc.abstractmethod - def execute(self, command_unit_list): - """ - execute(command_unit_list) -> status - """ - pass - @abc.abstractmethod - def get_command_result(self, command_id, condition): - """ - get_command_result(self, command_id, condition) -> (status,results) - """ - pass - -class QueryIF: - """ - class QueryIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def query(self, query_type, condition): - """ - query(self, query_type, condition) -> (status,results) - """ - pass - -class EventIF: - """ - class EventIF(object): - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def subscribe(self, event_type, condition): - """ - subscribe(self, event_type, condition) -> (status,subscribe_id) - """ - pass - @abc.abstractmethod - def unsubscribe(self, subscribe_id): - """ - unsubscribe(self,subscribe_id) -> status - """ - pass - @abc.abstractmethod - def get_event_detail(self, event_id, condition): - """ - get_event_detail(self,event_id,condition) -> (status,results) - """ - pass diff --git a/connection_test/scripts/pyrois/RoIS_HRI.pyc b/connection_test/scripts/pyrois/RoIS_HRI.pyc deleted file mode 100644 index 59dd27fdc82a83272b2284ff63f5763aa479cebc..0000000000000000000000000000000000000000 Binary files a/connection_test/scripts/pyrois/RoIS_HRI.pyc and /dev/null differ diff --git a/connection_test/scripts/pyrois/RoIS_Service.py b/connection_test/scripts/pyrois/RoIS_Service.py deleted file mode 100644 index 7cd57e5cbd2de7d75b03bc4bc4383d018bce2c11..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/RoIS_Service.py +++ /dev/null @@ -1,55 +0,0 @@ -# RoIS_Service.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 -# for Service Application - -"""RoIS_Service -""" - -import abc -import enum - -class Completed_Status(enum.Enum): - """Completed_Status - """ - OK = 1 - ERROR = 2 - ABORT = 3 - OUT_OF_RESOURCES = 4 - TIMEOUT = 5 - -class ErrorType(enum.Enum): - """ErrorType - """ - ENGINE_INTERNAL_ERROR = 1 - COMPONENT_INTERNEL_ERROR = 2 - COMPONENT_NOT_RESPONDING = 3 - USER_DEFINE_ERROR = 4 - -class Service_Application_Base: - """ - class ServiceApplicationBase(object) - """ - __metaclass__ = abc.ABCMeta - @abc.abstractmethod - def completed(self, command_id, status): - """ - connect(command_id, status) : void - """ - pass - @abc.abstractmethod - def notify_error(self, error_id, error_type): - """ - notify_error(error_id, error_type) : void - """ - pass - @abc.abstractmethod - def notify_event(self, event_id, event_type, subscribe_id, expire): - """ - notify_event(self, event_id, event_type, subscribe_id, expire) : void - """ - pass diff --git a/connection_test/scripts/pyrois/Service_Application_Base_example.py b/connection_test/scripts/pyrois/Service_Application_Base_example.py deleted file mode 100644 index 0d35004779147ab2de592a71f94ca595d78a57e6..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/Service_Application_Base_example.py +++ /dev/null @@ -1,100 +0,0 @@ -# Service_Application_Base_example.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 -# for HRI Engine - -"""Service_Application_Base_example -""" - -import sys -import queue -import time -import threading - -from pyrois import RoIS_Service - -if sys.version_info.major == 2: - import SocketServer - import SimpleXMLRPCServer - - class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): - """ThreadingXMLRPCServer - """ - pass -else: - import socketserver - import xmlrpc.server - - class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): - """ThreadingXMLRPCServer - """ - pass - - -class Service_Application_Base(RoIS_Service.Service_Application_Base): - """Service_Application_Base - """ - def __init__(self): - self.event_queue = queue.Queue() - - def poll_event(self): - """poll_event - """ - msg = self.event_queue.get() - return msg - - def completed(self, command_id, status): - msg = xmlrpc.client.dumps((command_id, status), 'completed') - self.event_queue.put(msg) - - def notify_error(self, error_id, error_type): - msg = xmlrpc.client.dumps((error_id, error_type), 'notify_error') - self.event_queue.put(msg) - - def notify_event(self, event_id, event_type, subscribe_id, expire): - msg = xmlrpc.client.dumps( - (event_id, event_type, subscribe_id, expire), 'notify_event') - self.event_queue.put(msg) - - -def event_dispatch(sa): - """event_dispatch - """ - sa.completed("0", RoIS_Service.Completed_Status.OK.value) - time.sleep(0.1) - sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) - time.sleep(0.2) - sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") - - -# def event_dispatch_long(sa): -# sa.completed("0", RoIS_Service.Completed_Status.OK.value) -# time.sleep(1) -# sa.notify_error("0", RoIS_Service.ErrorType.ENGINE_INTERNAL_ERROR.value) -# time.sleep(60) -# sa.notify_event("0", "sensor", "0", "2100-01-01T00:00:01+09:00") - -def example_sa(port): - """example_sa - """ - sa = Service_Application_Base() - - # start the timer to dispatch events - t = threading.Timer(0.1, event_dispatch, args=(sa,)) - t.start() - - # start the XML-RPC server - server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) - server.register_instance(sa) - server.register_introspection_functions() - server.register_multicall_functions() - # print("server running") - server.serve_forever() - - -if __name__ == '__main__': - example_sa(8000) diff --git a/connection_test/scripts/pyrois/Service_Application_IF.py b/connection_test/scripts/pyrois/Service_Application_IF.py deleted file mode 100644 index c1ca652d6aa90e49b10fccfc8cac43aa08fc0cd5..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/Service_Application_IF.py +++ /dev/null @@ -1,81 +0,0 @@ -# Service_Application_IF.py -# -# Copyright 2017 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 -# for Service Application - -"""Service_Application_IF -""" - -import xmlrpc.client -import logging -import logging.handlers - -from pyrois import RoIS_Service - - -class Service_Application_IF(RoIS_Service.Service_Application_Base): - """Service_Application_IF - """ - def __init__(self, uri, logger=None): - self._uri = uri - self._proxy = xmlrpc.client.ServerProxy(self._uri) - if logger is not None: - self.logger = logger - while True: - self.poll_event() - - def poll_event(self): - """poll_event - """ - # print("poll") - msg = self._proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - # print(params,methodname) - if methodname == 'completed': - self.completed(params[0], params[1]) - elif methodname == 'notify_error': - self.notify_error(params[0], params[1]) - elif methodname == 'notify_event': - self.notify_event(params[0], params[1], params[2], params[3]) - - def completed(self, command_id, status): - self.logger.debug('received completed event' - + command_id - + RoIS_Service.Completed_Status(status).name) - - def notify_error(self, error_id, error_type): - self.logger.debug('received error event' - + error_id - + RoIS_Service.ErrorType(error_type).name) - - def notify_event(self, event_id, event_type, subscribe_id, expire): - self.logger.debug('received event' - + event_id - + event_type - + subscribe_id - + expire) - - -def example_sa_IF(url, q): - try: - logger = logging.getLogger('Service_Application_IF') - logger.setLevel(logging.DEBUG) - ch = logging.handlers.QueueHandler(q) - # ch = logging.StreamHandler() - ch.setLevel(logging.DEBUG) - formatter = logging.Formatter( - '%(asctime)s - %(name)s - %(levelname)s - %(message)s') - ch.setFormatter(formatter) - logger.addHandler(ch) - a = Service_Application_IF(url, logger=logger) - except KeyboardInterrupt: - print("Interrupted") - - -if __name__ == '__main__': - pass diff --git a/connection_test/scripts/pyrois/__init__.py b/connection_test/scripts/pyrois/__init__.py deleted file mode 100644 index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000 diff --git a/connection_test/scripts/pyrois/__init__.pyc b/connection_test/scripts/pyrois/__init__.pyc deleted file mode 100644 index fbe0aff4f0b3d3761fc8169bbcdc88515888c72d..0000000000000000000000000000000000000000 Binary files a/connection_test/scripts/pyrois/__init__.pyc and /dev/null differ diff --git a/connection_test/scripts/pyrois/unittest.py b/connection_test/scripts/pyrois/unittest.py deleted file mode 100644 index b6dfeaec5ebc440c39868966b7b285940f827c0b..0000000000000000000000000000000000000000 --- a/connection_test/scripts/pyrois/unittest.py +++ /dev/null @@ -1,231 +0,0 @@ -# unittest.py -# -# Copyright 2018 Eiichi Inohira -# This software may be modified and distributed under the terms -# of the MIT license -# -# for python 3 - -"""unittest -""" - -import time -from multiprocessing import Process, Queue -import unittest -import http -import xmlrpc.client -import sysconfig - -from pyrois import RoIS_HRI, RoIS_Common -from pyrois import Service_Application_IF, Service_Application_Base_example -from pyrois import HRI_Engine_client, HRI_Engine_example -from pyrois import Person_Detection_client, Person_Detection - - -class TestServericeApplicationIF(unittest.TestCase): - """TestServericeApplicationIF - """ - def setUp(self): - # start the server process - self.ps = Process( - target=Service_Application_Base_example.example_sa, args=(8000,)) - self.ps.start() - time.sleep(0.1) - - def test_IF(self): - """ test_IF - """ - - q = Queue() - # start the client process - pc = Process(target=Service_Application_IF.example_sa_IF, - args=("http://127.0.0.1:8000/", q)) - pc.start() - - time.sleep(1) - - num_events = 0 - while not q.empty(): - msg = q.get() - num_events += 1 - # print(msg.asctime,msg.name,msg.levelname,msg.message) - - if pc.is_alive(): - pc.terminate() - - return self.assertEqual(num_events, 3) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - while self.ps.is_alive(): - time.sleep(0.1) - - -class TestHRIEngineIF(unittest.TestCase): - """TestHRIEngineIF - """ - - def setUp(self): - # start the server process - self.ps = Process(target=HRI_Engine_example.test_engine, args=(8000,)) - self.ps.start() - time.sleep(0.1) - - def test_IF(self): - """test_IF - """ - - with xmlrpc.client.ServerProxy("http://127.0.0.1:8000/") as proxy: - a = HRI_Engine_client.SystemIF(proxy) - b = HRI_Engine_client.CommandIF(proxy) - - try: - res = [ - a.connect(), - a.connect(), - b.bind(''), - a.disconnect(), - a.disconnect(), - b.bind('')] - # print(res) - except xmlrpc.client.ProtocolError as err: - print(err) - - return self.assertEqual(res, - [ - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - RoIS_HRI.ReturnCode_t.OK]) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - time.sleep(0.1) - - -class TestHRIEngineIF_integrated(unittest.TestCase): - """TestHRIEngineIF_integrated - """ - def setUp(self): - # start the server process - self.ps = Process(target=HRI_Engine_example.test_engine, args=(8000,)) - self.ps.start() - time.sleep(0.1) - - def test_IF(self): - """test_IF - """ - - try: - a = HRI_Engine_client.IF("http://127.0.0.1:8000/") - res = [ - a.connect(), - a.connect(), - a.get_profile(""), - a.get_error_detail("0", ""), - - a.search(""), - a.bind(""), - a.bind_any(""), - a.release(""), - a.get_parameter(""), - a.set_parameter("", ""), - a.execute(""), - a.get_command_result("", ""), - - a.query("", ""), - - a.subscribe("", ""), - a.unsubscribe(""), - a.get_event_detail("", ""), - - a.disconnect(), - a.disconnect(), - a.bind('')] - # print(res) - except xmlrpc.client.ProtocolError as err: - print(err) - - return self.assertEqual(res, - [RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - (RoIS_HRI.ReturnCode_t.OK, "Unsupported"), - (RoIS_HRI.ReturnCode_t.OK, "None"), - - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - (RoIS_HRI.ReturnCode_t.OK, "0"), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - - (RoIS_HRI.ReturnCode_t.OK, "0"), - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, ["None"]), - - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.ERROR, - RoIS_HRI.ReturnCode_t.OK]) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - time.sleep(0.1) - - -class TestPD(unittest.TestCase): - """TestPD - """ - - def setUp(self): - # start the server process - self.ps = Process(target=Person_Detection.example_pd, args=(8000,)) - self.ps.start() - time.sleep(0.5) - - def test_IF(self): - """test_IF - """ - a = Person_Detection_client.Person_Detection_Client("http://127.0.0.1:8000/") - try: - res = [ - a.start(), - a.stop(), - a.suspend(), - a.resume(), - a.component_status(), - len(a.events)] - except xmlrpc.client.ProtocolError as err: - print(err) - - return self.assertEqual(res, - [ - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - RoIS_HRI.ReturnCode_t.OK, - (RoIS_HRI.ReturnCode_t.OK, RoIS_Common.Component_Status.UNINITIALIZED), - 2]) - - def tearDown(self): - # terminate the server process - if self.ps.is_alive(): - self.ps.terminate() - time.sleep(0.1) - - -if __name__ == '__main__': - print("python", sysconfig.get_python_version()) - print("platform:", sysconfig.get_platform()) - unittest.main() diff --git a/connection_test/scripts/talker.py b/connection_test/scripts/talker.py deleted file mode 100755 index 8fed82446dcd4708e32f70a21b1aee3f39335993..0000000000000000000000000000000000000000 --- a/connection_test/scripts/talker.py +++ /dev/null @@ -1,138 +0,0 @@ -#!/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 deleted file mode 100644 index 603fe246f1e69231775853dad087f870cbe70ded..0000000000000000000000000000000000000000 --- a/connection_test/scripts/test.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/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()) diff --git a/goal_sender/CMakeLists.txt b/goal_sender/CMakeLists.txt deleted file mode 100644 index 5a822e325507aab9d35e9aed63e6abdbf34eff6a..0000000000000000000000000000000000000000 --- a/goal_sender/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(goal_sender) - -## 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 - actionlib - move_base_msgs - rospy -) - -## 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 -# move_base_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 goal_sender -# CATKIN_DEPENDS actionlib move_base_msgs rospy -# 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}/goal_sender.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/goal_sender_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_goal_sender.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/goal_sender/package.xml b/goal_sender/package.xml deleted file mode 100644 index d739126d6f519e2bc46742f9ab129a12c3082519..0000000000000000000000000000000000000000 --- a/goal_sender/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - goal_sender - 0.0.0 - The goal_sender package - - - - - tanacchi - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - actionlib - move_base_msgs - rospy - actionlib - move_base_msgs - rospy - actionlib - move_base_msgs - rospy - - - - - - - - diff --git a/goal_sender/scripts/test_component.py b/goal_sender/scripts/test_component.py deleted file mode 100644 index 73fdab2689dd6c5dc9e0f693649818ba57619401..0000000000000000000000000000000000000000 --- a/goal_sender/scripts/test_component.py +++ /dev/null @@ -1,9 +0,0 @@ -import time -from Navigation_client import Navigation_Client - -uri = "http://localhost:8001" -client = Navigation_Client(uri) - -print(client.start()) -time.sleep(3) -print(client.set_parameter("ahi", "chuni", "dwa")) diff --git a/goal_sender/scripts/test_engine.py b/goal_sender/scripts/test_engine.py deleted file mode 100644 index 006b76c90de2c4dc538cb73e9ae699af283ca7ee..0000000000000000000000000000000000000000 --- a/goal_sender/scripts/test_engine.py +++ /dev/null @@ -1,30 +0,0 @@ -# 1. launch navigation. -# 2. execute "rosrun (this_package) Navigation.py". -# 3. run route_guidance_engine server. -# 4. run this test. - - -from route_guidance_engine_client import IF -import time -from pyrois.RoIS_Common import Component_Status - -s = 'http://127.0.0.1:8000' -i = IF(s) -i.connect() -i.search("engine_profile_test") -print(i.bind('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis')) -#i.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:eng_test:SpeechSynthesis') -destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] -for dest in destinations: - print(i.set_parameter('Navigation',[dest, "", ""])) - navi_status = Component_Status.BUSY - print("TEST: status = {}".format(navi_status)) - while navi_status != Component_Status.READY: - server_status, navi_status = i.get_navi_status() - print("TEST: status = {}".format(navi_status)) - time.sleep(1) - time.sleep(3) - -#print(i.analysis_c_status()) -i.disconnect() -print("finish") diff --git a/rois_2dnav_gazebo/config/amcl_params.yaml b/rois_2dnav_gazebo/config/amcl_params.yaml index eb1cfbd3fdfa13c27a1f9213fc73dc84ef2806d4..dd2fe271fb234f4c07823a31da77b20049736a19 100644 --- a/rois_2dnav_gazebo/config/amcl_params.yaml +++ b/rois_2dnav_gazebo/config/amcl_params.yaml @@ -52,8 +52,4 @@ odom_alpha3: 0.3 odom_alpha4: 0.3 odom_alpha5: 0.1 -odom_frame_id: odom -base_frame_id: base_link -global_frame_id: map - tf_broadcast: true diff --git a/rois_2dnav_gazebo/config/costmap/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml similarity index 90% rename from rois_2dnav_gazebo/config/costmap/costmap_common_params.yaml rename to rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml index 63fa6eb7ed41d8e730ad21601fb7cd029ee7aa37..af297510a2160006a6a1a3d24ad4f4fb7269879c 100644 --- a/rois_2dnav_gazebo/config/costmap/costmap_common_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot1/costmap_common_params.yaml @@ -12,9 +12,9 @@ publish_voxel_map: true observation_sources: lrf_sensor lrf_sensor: { - sensor_frame: lrf_link, + sensor_frame: robot1/lrf_link, data_type: LaserScan, - topic: /scan, + topic: scan, # expected_update_rate: 1.0, observation_persistence: 0.0, marking: true, diff --git a/rois_2dnav_gazebo/config/costmap/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml similarity index 78% rename from rois_2dnav_gazebo/config/costmap/global_costmap_params.yaml rename to rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml index e503908e0c9485ae29b31f844f564237a3cf0eeb..f892f5584d0aacd6f1b2065a0ece3c10ddeeea66 100644 --- a/rois_2dnav_gazebo/config/costmap/global_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot1/global_costmap_params.yaml @@ -1,6 +1,6 @@ global_costmap: global_frame: map - robot_base_frame: base_link + robot_base_frame: robot1/base_footprint update_frequency: 3.0 publish_frequency: 3.0 static_map: true diff --git a/rois_2dnav_gazebo/config/costmap/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml similarity index 71% rename from rois_2dnav_gazebo/config/costmap/local_costmap_params.yaml rename to rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml index fe1fd30535a911f3b1670323ab86ac20723a35ac..d21d3bff3d3e0fdcb9a8425868f4c85ba701c73f 100644 --- a/rois_2dnav_gazebo/config/costmap/local_costmap_params.yaml +++ b/rois_2dnav_gazebo/config/costmap/robot1/local_costmap_params.yaml @@ -1,6 +1,6 @@ local_costmap: - global_frame: odom - robot_base_frame: base_link + global_frame: robot1/odom + robot_base_frame: robot1/base_footprint update_frequency: 8.0 publish_frequency: 7.0 width: 10.0 diff --git a/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6db6da4aba076eb98b91e3c2b43134fb34c96b54 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot2/costmap_common_params.yaml @@ -0,0 +1,25 @@ +obstacle_range: 5.0 +raytrace_range: 10.0 + +footprint: [[0.25, -0.18], [0.25, 0.18], [-0.15, 0.18], [-0.15, -0.18]] +inflation_radius: 0.3 +cost_scaling_factor: 10.0 + +origin_z: 0.0 +z_resolution: 0.02 +z_voxels: 0.02 +publish_voxel_map: true + +observation_sources: lrf_sensor +lrf_sensor: { + sensor_frame: robot2/lrf_link, + data_type: LaserScan, + topic: scan, + # expected_update_rate: 1.0, + observation_persistence: 0.0, + marking: true, + clearing: true, + min_obstacle_height: 0.2, + max_obstacle_height: 1.0, +} + diff --git a/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..a318075de7c61979ee797ec80df15fb869fbc385 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot2/global_costmap_params.yaml @@ -0,0 +1,9 @@ +global_costmap: + global_frame: map + robot_base_frame: robot2/base_footprint + update_frequency: 3.0 + publish_frequency: 3.0 + static_map: true + rolling_window: false + transform_tolerance: 1.0 + diff --git a/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8065ad90629f4baca230e76bb829914e3655ffba --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot2/local_costmap_params.yaml @@ -0,0 +1,11 @@ +local_costmap: + global_frame: robot2/odom + robot_base_frame: robot2/base_footprint + update_frequency: 8.0 + publish_frequency: 7.0 + width: 10.0 + height: 10.0 + resolution: 0.02 + meter_scoring: true + static_map: false + rolling_window: true diff --git a/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..119355e614dd6dee9a4a60a47d1acc51e1f49a40 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot3/costmap_common_params.yaml @@ -0,0 +1,25 @@ +obstacle_range: 5.0 +raytrace_range: 10.0 + +footprint: [[0.25, -0.18], [0.25, 0.18], [-0.15, 0.18], [-0.15, -0.18]] +inflation_radius: 0.3 +cost_scaling_factor: 10.0 + +origin_z: 0.0 +z_resolution: 0.02 +z_voxels: 0.02 +publish_voxel_map: true + +observation_sources: lrf_sensor +lrf_sensor: { + sensor_frame: robot3/lrf_link, + data_type: LaserScan, + topic: scan, + # expected_update_rate: 1.0, + observation_persistence: 0.0, + marking: true, + clearing: true, + min_obstacle_height: 0.2, + max_obstacle_height: 1.0, +} + diff --git a/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..9f0bd7e552b7be0621ba53da1f9a1a22287df6c8 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot3/global_costmap_params.yaml @@ -0,0 +1,9 @@ +global_costmap: + global_frame: map + robot_base_frame: robot3/base_footprint + update_frequency: 3.0 + publish_frequency: 3.0 + static_map: true + rolling_window: false + transform_tolerance: 1.0 + diff --git a/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml b/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml new file mode 100644 index 0000000000000000000000000000000000000000..2f5a5af986a1183dea8354a08a9722af84f66368 --- /dev/null +++ b/rois_2dnav_gazebo/config/costmap/robot3/local_costmap_params.yaml @@ -0,0 +1,11 @@ +local_costmap: + global_frame: robot3/odom + robot_base_frame: robot3/base_footprint + update_frequency: 8.0 + publish_frequency: 7.0 + width: 10.0 + height: 10.0 + resolution: 0.02 + meter_scoring: true + static_map: false + rolling_window: true diff --git a/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml b/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml index 669410df42bf007a8e509040f1813339fc6618e4..7459a1ab8b94d665ace62afbb314f9c3a3a69532 100644 --- a/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml +++ b/rois_2dnav_gazebo/config/planner/base_local_planner_params.yaml @@ -26,6 +26,6 @@ TrajectoryPlannerROS: # Show cost map publish_cost_grid_pc: true - global_frame_id: map + global_frame_id: /map meter_scoring: false diff --git a/rois_2dnav_gazebo/launch/amcl.launch b/rois_2dnav_gazebo/launch/amcl.launch index 3d9fb4fa495c7c063184aff90311c2bb46944173..9ae6cbbac129e77fd08b924114b13bb6b071c9fa 100644 --- a/rois_2dnav_gazebo/launch/amcl.launch +++ b/rois_2dnav_gazebo/launch/amcl.launch @@ -1,5 +1,35 @@ + + + + - + + + + + + + diff --git a/rois_2dnav_gazebo/launch/move_base.launch b/rois_2dnav_gazebo/launch/move_base.launch index bef6ff6486549f7fec108f41e1af2c3a4a8d5601..342ca9ef8bd62f1d3c3908452717370a3e9c6e55 100644 --- a/rois_2dnav_gazebo/launch/move_base.launch +++ b/rois_2dnav_gazebo/launch/move_base.launch @@ -1,18 +1,22 @@ + - - + + - - + + - + + + diff --git a/rois_2dnav_gazebo/rviz/multi_navigation.rviz b/rois_2dnav_gazebo/rviz/multi_navigation.rviz new file mode 100644 index 0000000000000000000000000000000000000000..eb286fc146c5418fa0d8b7396edb24f2a3d705a5 --- /dev/null +++ b/rois_2dnav_gazebo/rviz/multi_navigation.rviz @@ -0,0 +1,301 @@ +Panels: + - Class: rviz/Displays + Help Height: 85 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /LaserScan1 + - /Map2 + - /Path2 + - /PoseArray1 + Splitter Ratio: 0.29716193675994873 + Tree Height: 801 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + map: + Value: true + robot1/base_footprint: + Value: true + robot1/base_link: + Value: true + robot1/caster_link: + Value: true + robot1/left_wheel_link: + Value: true + robot1/lrf_link: + Value: true + robot1/odom: + Value: true + robot1/right_wheel_link: + Value: true + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + robot1/odom: + robot1/base_footprint: + robot1/base_link: + {} + robot1/caster_link: + {} + robot1/left_wheel_link: + {} + robot1/lrf_link: + {} + robot1/right_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /robot1/scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Map + Topic: /robot1/move_base/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /move_base/GlobalPlanner/plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 52; 101; 164 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /robot1/move_base/GlobalPlanner/plan + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: /move_base/local_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: /robot1/particlecloud + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 47.22295379638672 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 2.620417356491089 + Y: -0.4015696048736572 + Z: -3.6895456314086914 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.2697967290878296 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.1435768604278564 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000020a000003bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000047000003bd000000f100fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000047000002bf000000c200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000039f00000044fc0100000002fb0000000800540069006d006501000000000000039f0000037100fffffffb0000000800540069006d006501000000000000045000000000000000000000039f000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 927 + X: 67 + Y: 34 diff --git a/rois_bringup/launch/navigation.launch b/rois_bringup/launch/navigation.launch index 75644b21408656c0af253674520632f7bdff053d..6d2981a9199dd72627a1362f0610cd284e20fb46 100644 --- a/rois_bringup/launch/navigation.launch +++ b/rois_bringup/launch/navigation.launch @@ -2,17 +2,76 @@ + + - + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rois_description/robot/robot1.urdf.xacro b/rois_description/robot/robot1.urdf.xacro new file mode 100644 index 0000000000000000000000000000000000000000..bf0d8b5c3e3b4059bd462de840ae655403c07fae --- /dev/null +++ b/rois_description/robot/robot1.urdf.xacro @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + false + + 0.2 0 0 0 0 0 + true + 40 + + + + 500 + 1 + -${M_PI*3/4} + ${M_PI*3/4} + + + + 0.20 + 131 + 0.01 + + + + scan + lrf_link + 0.9 + 130 + + + + + + + + robot1 + + + + + true + Gazebo/White + + + + + + true + Gazebo/Green + + + + + + true + Gazebo/Red + + + + + + true + Gazebo/Black + + + + + diff --git a/rois_description/robot/robot2.urdf.xacro b/rois_description/robot/robot2.urdf.xacro new file mode 100644 index 0000000000000000000000000000000000000000..52d45ef85c3a67dafdf7c5a59b4bfd62e2b643cb --- /dev/null +++ b/rois_description/robot/robot2.urdf.xacro @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + false + + 0.2 0 0 0 0 0 + true + 40 + + + + 500 + 1 + -${M_PI*3/4} + ${M_PI*3/4} + + + + 0.20 + 131 + 0.01 + + + + scan + lrf_link + 0.9 + 130 + + + + + + + + robot2 + + + + + true + Gazebo/White + + + + + + true + Gazebo/Green + + + + + + true + Gazebo/Red + + + + + + true + Gazebo/Black + + + + + diff --git a/rois_description/robot/robot3.urdf.xacro b/rois_description/robot/robot3.urdf.xacro new file mode 100644 index 0000000000000000000000000000000000000000..228362ddeadf9e2d32ed681aa1e758b029895e2c --- /dev/null +++ b/rois_description/robot/robot3.urdf.xacro @@ -0,0 +1,234 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 15 + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + false + + 0.2 0 0 0 0 0 + true + 40 + + + + 500 + 1 + -${M_PI*3/4} + ${M_PI*3/4} + + + + 0.20 + 131 + 0.01 + + + + scan + lrf_link + 0.9 + 130 + + + + + + + + robot3 + + + + + true + Gazebo/White + + + + + + true + Gazebo/Green + + + + + + true + Gazebo/Red + + + + + + true + Gazebo/Black + + + + + diff --git a/rois_description/robot/roisbot.urdf.xacro b/rois_description/robot/roisbot.urdf.xacro index b201f413dceede53369518817832a06dacef9c7e..01f4b7a3ee37cf8a9ab327b6ef7ba457bcf58005 100644 --- a/rois_description/robot/roisbot.urdf.xacro +++ b/rois_description/robot/roisbot.urdf.xacro @@ -1,12 +1,12 @@ - - - - - - - + + + + + + + diff --git a/rois_gazebo/config/controller.yaml b/rois_gazebo/config/controller.yaml index d9ae17569223d3b43dbd0401a78a5e9910490d94..0c663c230062e03bf630c8cda99928540747af4c 100644 --- a/rois_gazebo/config/controller.yaml +++ b/rois_gazebo/config/controller.yaml @@ -1,46 +1,45 @@ -roisbot: - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 - diff_drive_controller: - type : "diff_drive_controller/DiffDriveController" - left_wheel : 'left_wheel_joint' - right_wheel : 'right_wheel_joint' - pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - - # Wheel separation and diameter. These are both optional. - # diff_drive_controller will attempt to read either one or both from the - # URDF if not specified as a parameter - wheel_separation : 0.3 - wheel_radius : 0.1 - - # Wheel separation and radius multipliers - wheel_separation_multiplier: 1.0 # default: 1.0 - wheel_radius_multiplier : 1.0 # default: 1.0 - - # Velocity commands timeout [s], default 0.5 - cmd_vel_timeout: 1.0 - - # Base frame_id - base_frame_id: base_footprint #default: base_link - - # Velocity and acceleration limits - # Whenever a min_* is unspecified, default to -max_* - linear: - x: - has_velocity_limits : true - max_velocity : 2.0 # m/s - min_velocity : -2.0 # m/s - has_acceleration_limits: true - max_acceleration : 2.0 # m/s^2 - min_acceleration : -2.0 # m/s^2 - angular: - z: - has_velocity_limits : true - max_velocity : 1.0 # rad/s - min_velocity : -1.0 - has_acceleration_limits: true - max_acceleration : 1.0 # rad/s^2 - min_acceleration : -1.0 +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: base_footprint #default: base_link + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/config/robot1_controller.yaml b/rois_gazebo/config/robot1_controller.yaml new file mode 100644 index 0000000000000000000000000000000000000000..fb8780b1d1c4393b32a465feed73c4c37f41c634 --- /dev/null +++ b/rois_gazebo/config/robot1_controller.yaml @@ -0,0 +1,46 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: robot1/base_footprint #default: base_link + odom_frame_id: robot1/odom + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/config/robot2_controller.yaml b/rois_gazebo/config/robot2_controller.yaml new file mode 100644 index 0000000000000000000000000000000000000000..56eabf73509f804722e02f44f9428ffc762e8683 --- /dev/null +++ b/rois_gazebo/config/robot2_controller.yaml @@ -0,0 +1,46 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: robot2/base_footprint #default: base_link + odom_frame_id: robot2/odom + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/config/robot3_controller.yaml b/rois_gazebo/config/robot3_controller.yaml new file mode 100644 index 0000000000000000000000000000000000000000..e4c01859af5cd5527ef4384d191a3e86a8056be9 --- /dev/null +++ b/rois_gazebo/config/robot3_controller.yaml @@ -0,0 +1,46 @@ +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +diff_drive_controller: + type : "diff_drive_controller/DiffDriveController" + left_wheel : 'left_wheel_joint' + right_wheel : 'right_wheel_joint' + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + # Wheel separation and diameter. These are both optional. + # diff_drive_controller will attempt to read either one or both from the + # URDF if not specified as a parameter + wheel_separation : 0.3 + wheel_radius : 0.1 + + # Wheel separation and radius multipliers + wheel_separation_multiplier: 1.0 # default: 1.0 + wheel_radius_multiplier : 1.0 # default: 1.0 + + # Velocity commands timeout [s], default 0.5 + cmd_vel_timeout: 1.0 + + # Base frame_id + base_frame_id: robot3/base_footprint #default: base_link + odom_frame_id: robot3/odom + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + has_velocity_limits : true + max_velocity : 2.0 # m/s + min_velocity : -2.0 # m/s + has_acceleration_limits: true + max_acceleration : 2.0 # m/s^2 + min_acceleration : -2.0 # m/s^2 + angular: + z: + has_velocity_limits : true + max_velocity : 1.0 # rad/s + min_velocity : -1.0 + has_acceleration_limits: true + max_acceleration : 1.0 # rad/s^2 + min_acceleration : -1.0 diff --git a/rois_gazebo/launch/setup_gazebo.launch b/rois_gazebo/launch/setup_gazebo.launch index 8b2b02db26257ad9692b964c1fb3ef9e6e4e646f..95e9dfdb0d4d19be77bad385f3b2110662cc04d3 100644 --- a/rois_gazebo/launch/setup_gazebo.launch +++ b/rois_gazebo/launch/setup_gazebo.launch @@ -1,13 +1,13 @@ - + - - - - - + + + + + - + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rois_gazebo/launch/teleop_keyboard.launch b/rois_gazebo/launch/teleop_keyboard.launch index ad051e88cb750f9f58e8a519cc74e384f3616ac6..467546c366bba6f5eabbe2ed51c1fe8d04b22f11 100644 --- a/rois_gazebo/launch/teleop_keyboard.launch +++ b/rois_gazebo/launch/teleop_keyboard.launch @@ -1,5 +1,6 @@ + + to="/$(arg robot_name)/diff_drive_controller/cmd_vel" /> diff --git a/route_guidance_ros/CMakeLists.txt b/route_guidance_ros/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..d9db1fb24772fcfd912c6c05a84c17d5dded0a21 --- /dev/null +++ b/route_guidance_ros/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8.3) +project(route_guidance_ros) + +find_package(catkin REQUIRED COMPONENTS + actionlib + geometry_msgs + move_base_msgs + rospy +) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES route_guidance_ros +# CATKIN_DEPENDS actionlib geometry_msgs move_base_msgs rospy +# DEPENDS system_lib +) + +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) diff --git a/route_guidance_ros/package.xml b/route_guidance_ros/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..fbc067264cfec9614f69c6488fe5177b2206ed14 --- /dev/null +++ b/route_guidance_ros/package.xml @@ -0,0 +1,26 @@ + + + route_guidance_ros + 0.0.0 + The route_guidance_ros package + + tanacchi + + MIT + + catkin + actionlib + geometry_msgs + move_base_msgs + rospy + actionlib + geometry_msgs + move_base_msgs + rospy + actionlib + geometry_msgs + move_base_msgs + rospy + + + diff --git a/goal_sender/scripts/Navigation.py b/route_guidance_ros/scripts/Navigation.py similarity index 86% rename from goal_sender/scripts/Navigation.py rename to route_guidance_ros/scripts/Navigation.py index 879b2bdf8c7a11ec795d972c0b18518041934406..fb38f75cab2922e85ac0d5314353937ab202bb93 100755 --- a/goal_sender/scripts/Navigation.py +++ b/route_guidance_ros/scripts/Navigation.py @@ -76,10 +76,10 @@ class Event(RoIS_Common.Event): from goal_sender_ros import GoalSenderROS class Navigation(Event, Command, Query): - def __init__(self, c): + def __init__(self, c, robot_name): super(Navigation, self).__init__(c) self._component = c - self._goal_sender = GoalSenderROS() + self._goal_sender = GoalSenderROS(robot_name) self._component.Target_Position = [""] self._component.Time_Limit = 10 self._component.Routing_Policy = "" @@ -106,10 +106,10 @@ class Component(object): import rospy -def example_n(port): +def example_n(port, robot_name): print("Starting node...") component = Component() - n = Navigation(component) + n = Navigation(component, robot_name) print("Navigation constructed") # start the timer to dispatch events @@ -125,5 +125,10 @@ def example_n(port): if __name__ == '__main__': - rospy.init_node('navigation_component') - example_n(8001) + if len(sys.argv) < 3: + print("rosrun Navigation.py ") + raise RuntimeError + + port, robot_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('navigation_component', anonymous=True) + example_n(port=port, robot_name=robot_name) diff --git a/goal_sender/scripts/Navigation_client.py b/route_guidance_ros/scripts/Navigation_client.py similarity index 100% rename from goal_sender/scripts/Navigation_client.py rename to route_guidance_ros/scripts/Navigation_client.py diff --git a/route_guidance_ros/scripts/System_Information.py b/route_guidance_ros/scripts/System_Information.py new file mode 100755 index 0000000000000000000000000000000000000000..a2493cc13731ed606f8a1355b792b24ee4a2bf68 --- /dev/null +++ b/route_guidance_ros/scripts/System_Information.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python + +# System_Information.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +"""System_Information +""" + +import sys +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import Queue as queue + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import queue + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + """ThreadingXMLRPCServer + """ + pass + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, c): + super(Query, self).__init__() + self._component = c + + def component_status(self): + pass + + def robot_position(self): + pass + + def engine_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + e_status = RoIS_Common.Component_Status.READY.value + operable_time = ["2100-01-01T00:00:01+09:00","2100-01-01T00:00:01+09:00"] + return (status, e_status, operable_time) + + +class System_Information(Query): + """System_Information + """ + def __init__(self, c): + super(System_Information, self).__init__(c) + self._component = c + + def robot_position(self): + status = RoIS_HRI.ReturnCode_t.OK.value + timestamp = str(datetime.now()) + robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::robot1"] # codebook, version + position_data = self._component.get_position() + return (status, timestamp, robot_ref, position_data) + + +import rospy +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped + +class component: + """component + """ + def __init__(self, robot_name): + self._state = False + self.position = None + rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback) + + def amcl_pose_callback(self, amcl_pose): + pos = amcl_pose.pose.pose.position + self.position = [pos.x, pos.y] + + def get_position(self): + return self.position + +def event_dispatch(pd): + """event_dispatch + """ + +def example_si(port, robot_name): + """example_si + """ + c = component(robot_name) + si = System_Information(c) + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(si,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(si) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 3: + print("rosrun System_Information.py ") + raise RuntimeError + + port, robot_name = int(sys.argv[1]), sys.argv[2] + rospy.init_node('sysinfo_component', anonymous=True) + example_si(port, robot_name) diff --git a/route_guidance_ros/scripts/System_Information_client.py b/route_guidance_ros/scripts/System_Information_client.py new file mode 100644 index 0000000000000000000000000000000000000000..d44ddc1028366997ef9ce47ae55562d875fe757d --- /dev/null +++ b/route_guidance_ros/scripts/System_Information_client.py @@ -0,0 +1,41 @@ +# System_Information_Client.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Engine + +"""System_Information_Client +""" + +import threading +# import logging +import xmlrpc.client + +from pyrois import RoIS_Common, RoIS_HRI + + +class Query(RoIS_Common.Query): + """Query + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) + + def robot_position(self): + (status, timestamp, robot_ref, position_data) = self._proxy.robot_position() + return (RoIS_HRI.ReturnCode_t(status), timestamp, robot_ref, position_data) + + def engine_status(self): + (status, e_status, operable_time) = self._proxy.component_status() + return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(e_status), operable_time) + + +class System_Information_Client(Query): + """System_Information_Client + """ + def __init__(self, uri): + self._uri = uri + self._proxy = xmlrpc.client.ServerProxy(self._uri) diff --git a/route_guidance_ros/scripts/VirtualNavigation.py b/route_guidance_ros/scripts/VirtualNavigation.py new file mode 100755 index 0000000000000000000000000000000000000000..056888befdfc476aed8163abce0c114cf859146a --- /dev/null +++ b/route_guidance_ros/scripts/VirtualNavigation.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python + +# VirtualNavigation.py +# +# Copyright 2018 Eiichi Inohira +# This software may be modified and distributed under the terms +# of the MIT license +# +# For python3 +# For HRI Component + +import sys +# import queue +import time +from datetime import datetime +import threading + +from pyrois import RoIS_Common, RoIS_HRI + +if sys.version_info.major == 2: + import Queue as queue + import SocketServer + import SimpleXMLRPCServer + + class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer): + pass +else: + import queue + import socketserver + import xmlrpc.server + + class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer): + pass + + +class Command(RoIS_Common.Command): + def __init__(self, c): + self._component = c + + def start(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def stop(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def suspend(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def resume(self): + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + +class Query(RoIS_Common.Query): + def __init__(self, c): + self._component = c + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = RoIS_Common.Component_Status.UNINITIALIZED.value + return (status, c_status) + + +class Event(RoIS_Common.Event): + def __init__(self, c): + self._component = c + self.event_queue = queue.Queue() + + # def poll_event(self): + # msg = self.event_queue.get() + # return msg + +from goal_sender_ros import GoalSenderROS + +class VirtualNavigation(Event, Command, Query): + def __init__(self, c, robot_names): + super(VirtualNavigation, self).__init__(c) + self._component = c + self._component.Target_Position = [""] + self._component.Time_Limit = 10 + self._component.Routing_Policy = "" + + def set_parameter(self, target_position, time_limit, routing_policy): + self._component.send_goal_to_nearest_robot(target_position) + status = RoIS_HRI.ReturnCode_t.OK.value + return status + + def component_status(self): + status = RoIS_HRI.ReturnCode_t.OK.value + c_status = self._goal_sender.get_goalsender_state().value + return (status, c_status) + + +def event_dispatch(n): + # n.person_detected(datetime.now().isoformat(), 1) + time.sleep(0.1) + # n.person_detected(datetime.now().isoformat(), 1) + +import numpy as np +import rospy +from geometry_msgs.msg import PoseWithCovarianceStamped + +from functools import partial + +class Component(object): + def __init__(self, robot_names): + self.__robot_names = robot_names + self.__goal_senders = \ + {robot_name : GoalSenderROS(robot_name) for robot_name in robot_names} + self.__robot_positions = {robot_name : None for robot_name in robot_names} + for robot_name in robot_names: + rospy.Subscriber(robot_name+"/amcl_pose", + PoseWithCovarianceStamped, + partial(self.update_position, robot_name)) + + + def update_position(self, robot_name, amcl_pose): + pos = amcl_pose.pose.pose.position + self.__robot_positions[robot_name] = np.array([pos.x, pos.y]) + + + def send_goal_to_nearest_robot(self, dest): + dest = np.array(dest) + distance_table = \ + {name: np.linalg.norm(dest - pos) for name, pos in self.__robot_positions.items()} + nearest_robot = min(distance_table, key=distance_table.get) + print("nearest_robot is ", nearest_robot) + self.__goal_senders[nearest_robot].send_goal(dest.tolist()) + + +def example_n(port, robot_names): + print("Starting node...") + component = Component(robot_names) + n = VirtualNavigation(component, robot_names) + print("VirtualNavigation constructed") + + # start the timer to dispatch events + t = threading.Timer(0.1, event_dispatch, args=(n,)) + t.start() + + # start the XML-RPC server + server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False) + server.register_instance(n) + server.register_introspection_functions() + server.register_multicall_functions() + server.serve_forever() + + +if __name__ == '__main__': + if len(sys.argv) < 3: + print("rosrun VirtualNavigation.py ...") + raise RuntimeError + + port, robot_names = int(sys.argv[1]), sys.argv[2:] + rospy.init_node('virtual_navigation_component', anonymous=True) + example_n(port=port, robot_names=robot_names) diff --git a/connection_test/scripts/pyrois/Person_Detection_client.py b/route_guidance_ros/scripts/VirtualNavigation_client.py similarity index 52% rename from connection_test/scripts/pyrois/Person_Detection_client.py rename to route_guidance_ros/scripts/VirtualNavigation_client.py index 77f4af4b204237003d69ddddd830e188dfaf133a..be14ab33d157a3c6cb13b22fff2fa61e70bf6ed5 100644 --- a/connection_test/scripts/pyrois/Person_Detection_client.py +++ b/route_guidance_ros/scripts/VirtualNavigation_client.py @@ -1,4 +1,4 @@ -# Person_Detection_Client.py +# VirtualNavigation_Client.py # # Copyright 2018 Eiichi Inohira # This software may be modified and distributed under the terms @@ -7,7 +7,7 @@ # For python3 # For HRI Engine -"""Person_Detection_Client +"""VirtualNavigation_Client """ import threading @@ -40,6 +40,11 @@ class Command(RoIS_Common.Command): status = RoIS_HRI.ReturnCode_t(self._proxy.resume()) return status + def set_parameter(self, target_position, time_limit, routing_policy): + status = self._proxy.set_parameter(target_position, time_limit, routing_policy) + status = RoIS_HRI.ReturnCode_t(status) + return status + class Query(RoIS_Common.Query): """Query @@ -49,8 +54,12 @@ class Query(RoIS_Common.Query): self._proxy = xmlrpc.client.ServerProxy(self._uri) def component_status(self): - (status, c_status) = self._proxy.component_status() + status, c_status = self._proxy.component_status() return (RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status)) + + def get_parameter(self): + (status, target_position, time_limit, routing_policy) = self._proxy.get_parameter() + return (RoIS_HRI.ReturnCode_t(status), target_position, time_limit, routing_policy) class Event(RoIS_Common.Event): @@ -59,46 +68,40 @@ class Event(RoIS_Common.Event): def __init__(self, uri): self._uri = uri self._e_proxy = xmlrpc.client.ServerProxy(self._uri) + self.events = [] # if logger is not None: # self.logger = logger - - def start_th(self): - self.th = threading.Thread(target=self.event_loop) - self.th.start() - - def event_loop(self): - """event_loop - """ - while True: - try: - self.poll_event() - except ConnectionRefusedError: - break - - def poll_event(self): - """poll_event - """ - msg = self._e_proxy.poll_event() - (params, methodname) = xmlrpc.client.loads(msg) - #self.logger.debug('poll_event: '+methodname) - # print(params,methodname) - if methodname == 'person_detected': - self.person_detected(params[0], params[1]) - - def person_detected(self, timestamp, number): - self.events.append((timestamp,number)) - print("person_detected",timestamp,number) - # self.logger.debug('received completed event' - # + command_id - # + RoIS_Service.Completed_Status(status).name) - - -class Person_Detection_Client(Command, Query, Event): - """Person_Detection_Client + + # def start_th(self): + # self.th = threading.Thread(target=self.event_loop) + # self.th.start() + + # def event_loop(self): + # """event_loop + # """ + # while True: + # try: + # self.poll_event() + # except ConnectionRefusedError: + # break + + # def poll_event(self): + # """poll_event + # """ + # msg = self._e_proxy.poll_event() + # (params, methodname) = xmlrpc.client.loads(msg) + # #self.logger.debug('poll_event: '+methodname) + # print(params,methodname) + # if methodname == 'speech_recognized': + # self.speech_recognized(params[0], params[1]) + + +class VirtualNavigation_Client(Command, Query, Event): + """VirtualNavigation_Client """ def __init__(self, uri): self._uri = uri self._proxy = xmlrpc.client.ServerProxy(self._uri) self._e_proxy = xmlrpc.client.ServerProxy(self._uri) self.events = [] - self.start_th() + # self.start_th() diff --git a/goal_sender/scripts/goal_sender_ros.py b/route_guidance_ros/scripts/goal_sender_ros.py similarity index 92% rename from goal_sender/scripts/goal_sender_ros.py rename to route_guidance_ros/scripts/goal_sender_ros.py index c6058e48db97d7733ca4d534cd5fa807cad86d65..3b69730de70dcd80a734b54481cd6971c7dbb20c 100644 --- a/goal_sender/scripts/goal_sender_ros.py +++ b/route_guidance_ros/scripts/goal_sender_ros.py @@ -38,8 +38,8 @@ class GoalSenderROS(object): g_state.LOST: c_state.ERROR } - def __init__(self): - self.action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction) + def __init__(self, robot_name): + self.action_client = actionlib.SimpleActionClient(robot_name+'/move_base', MoveBaseAction) self.action_client.wait_for_server() def send_goal(self, pose_xy): diff --git a/goal_sender/scripts/route_guidance_engine.py b/route_guidance_ros/scripts/sub_engine.py similarity index 88% rename from goal_sender/scripts/route_guidance_engine.py rename to route_guidance_ros/scripts/sub_engine.py index 335976b1be46521ba9339e197cf0cfa2410f4106..638b0f791be9d24bccc341bbe89c717493ea7fd5 100644 --- a/goal_sender/scripts/route_guidance_engine.py +++ b/route_guidance_ros/scripts/sub_engine.py @@ -1,4 +1,4 @@ -# route_guidance_engine.py +# sub_engine.py # # Copyright 2017 Eiichi Inohira # This software may be modified and distributed under the terms @@ -7,7 +7,7 @@ # For python 3 # For HRI Engine -"""route_guidance_engine +"""sub_engine """ from __future__ import print_function @@ -167,15 +167,17 @@ class EventIF(RoIS_HRI.EventIF): from Navigation_client import Navigation_Client as NavClient +from System_Information_client import System_Information_Client as SysClient class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): """IF """ - def __init__(self, Engine): + def __init__(self, Engine, engine_port): super().__init__(Engine) self.command_id = 0 self.compoent_clients = { - 'Navigation': NavClient('http://localhost:8001') + 'Navigation': NavClient('http://localhost:' + str(engine_port+1)), + 'System_Information': SysClient('http://localhost:' + str(engine_port+2)) } def set_parameter(self, component_ref, parameters): @@ -192,6 +194,11 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application def get_navi_status(self): status, c_status = self.compoent_clients['Navigation'].component_status() return (status.value, c_status.value) + + def get_position(self): + si_compoent = self.compoent_clients['System_Information'] + status, timestamp, robot_ref, position_data = si_compoent.robot_position() + return (status.value, timestamp, robot_ref, position_data) ############## class IF_server: @@ -221,8 +228,13 @@ class RouteGuidanceEngine: def test_engine(port): """test_engine """ - IF_server(port).run(IF(RouteGuidanceEngine())) + IF_server(port).run(IF(RouteGuidanceEngine(), port)) if __name__ == "__main__": - test_engine(8000) + if len(sys.argv) < 2: + print("py sub_engine.py ") + raise RuntimeError + + port = int(sys.argv[1]) + test_engine(port) diff --git a/goal_sender/scripts/route_guidance_engine_client.py b/route_guidance_ros/scripts/sub_engine_client.py similarity index 93% rename from goal_sender/scripts/route_guidance_engine_client.py rename to route_guidance_ros/scripts/sub_engine_client.py index 6abe2d974ee306c75e95e5758a6bb5afe95077d9..0fc6e7ece7b008f0f58d450135182e38db52cb4a 100644 --- a/goal_sender/scripts/route_guidance_engine_client.py +++ b/route_guidance_ros/scripts/sub_engine_client.py @@ -14,6 +14,7 @@ import xmlrpc.client # from pyrois import Service_Application_Base_example from pyrois import RoIS_HRI, RoIS_Common, RoIS_Service +from pyrois.Service_Application_IF import Service_Application_IF # import Service_Application_IF_test class SystemIF(RoIS_HRI.SystemIF): @@ -120,7 +121,7 @@ class EventIF(RoIS_HRI.EventIF): return (status, results) -class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application_Base): +class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_IF): """IF """ def __init__(self, uri): @@ -138,4 +139,9 @@ class IF(SystemIF, CommandIF, QueryIF, EventIF, RoIS_Service.Service_Application def get_navi_status(self): status, c_status = self._proxy.get_navi_status() return RoIS_HRI.ReturnCode_t(status), RoIS_Common.Component_Status(c_status) + + def get_position(self): + position = self._proxy.get_position() + position[0] = RoIS_HRI.ReturnCode_t(position[0]) + return position ############## diff --git a/route_guidance_ros/scripts/test_nav_component.py b/route_guidance_ros/scripts/test_nav_component.py new file mode 100644 index 0000000000000000000000000000000000000000..8724364d6d422fe0835685c5f8d5eaf4921901ac --- /dev/null +++ b/route_guidance_ros/scripts/test_nav_component.py @@ -0,0 +1,16 @@ +# 1. launch single navigation. +# 2. run this test. + + +import time +from Navigation_client import Navigation_Client +from utilities import setup_single_robot + +process = setup_single_robot() + +uri = "http://localhost:8011" +client = Navigation_Client(uri) + +print(client.start()) +time.sleep(3) +print(client.set_parameter([10, 0], "", "")) diff --git a/route_guidance_ros/scripts/test_sub_engine.py b/route_guidance_ros/scripts/test_sub_engine.py new file mode 100644 index 0000000000000000000000000000000000000000..a8724fc6d6706e50753911ba30507a3deb8a5732 --- /dev/null +++ b/route_guidance_ros/scripts/test_sub_engine.py @@ -0,0 +1,29 @@ +# 1. launch single navigation. +# 2. run this test. + + +from sub_engine_client import IF as Engine +import time +from pyrois.RoIS_Common import Component_Status +from utilities import setup_single_robot + +process = setup_single_robot() + +engine = Engine('http://127.0.0.1:8010') +engine.connect() +engine.search("engine_profile_test") +destinations = [[5, 0], [10, 0], [25, 0], [25, 10]] +for dest in destinations: + print(engine.set_parameter('Navigation',[dest, "", ""])) + navi_status = Component_Status.BUSY + while navi_status != Component_Status.READY: + server_status, navi_status = engine.get_navi_status() + print("TEST: status = {}".format(navi_status)) + time.sleep(1) + position = engine.get_position() + print("TEST: position = {}".format(position)) + time.sleep(3) + +#print(engine.analysis_c_status()) +engine.disconnect() +print("finish") diff --git a/route_guidance_ros/scripts/test_sys_component.py b/route_guidance_ros/scripts/test_sys_component.py new file mode 100644 index 0000000000000000000000000000000000000000..556e5ff7207b8e14eb3a6d27a697201699476491 --- /dev/null +++ b/route_guidance_ros/scripts/test_sys_component.py @@ -0,0 +1,17 @@ +# 1. launch single navigation. +# 2. run this test. + + +import time +from System_Information_client import System_Information_Client +from utilities import setup_single_robot + +process = setup_single_robot() + +uri = "http://localhost:8012" +client = System_Information_Client(uri) + +start_time = time.time() +while time.time() - start_time < 10: + time.sleep(1) + print(client.robot_position()) diff --git a/route_guidance_ros/scripts/test_toplevel_engine.py b/route_guidance_ros/scripts/test_toplevel_engine.py new file mode 100644 index 0000000000000000000000000000000000000000..36e2654a8ce99feeef05041444a37d468ffb471a --- /dev/null +++ b/route_guidance_ros/scripts/test_toplevel_engine.py @@ -0,0 +1,20 @@ +# 1. launch multi navigation. +# 2. run this test. + + +from toplevel_engine_client import IF as Engine +import time +from pyrois.RoIS_Common import Component_Status +from utilities import setup_multi_robot + +process = setup_multi_robot() + +engine = Engine('http://127.0.0.1:8000') +engine.connect() +for dest in [[25, 0], [0, 20], [5, 0]]: + status = engine.set_parameter('Navigation', [dest, "", ""]) + print(status) + time.sleep(5) + +engine.disconnect() +print("finish") diff --git a/route_guidance_ros/scripts/test_vnav_component.py b/route_guidance_ros/scripts/test_vnav_component.py new file mode 100644 index 0000000000000000000000000000000000000000..9fbdc7eae5223c497092d9779dc0d10605e300ba --- /dev/null +++ b/route_guidance_ros/scripts/test_vnav_component.py @@ -0,0 +1,26 @@ +# 1. launch multi navigation. +# 2. run this test. + + +from toplevel_engine_client import IF as Engine +import time +from pyrois.RoIS_Common import Component_Status +from utilities import setup_multi_robot, SubprocessWrapper + +process = setup_multi_robot() +command = [ + "rosrun", "route_guidance_ros", "VirtualNavigation.py", + "8041", "robot1", "robot2", "robot3" +] +process.append(SubprocessWrapper(command)) +time.sleep(10) + +engine = Engine('http://127.0.0.1:8000') +engine.connect() +for dest in [[25, 0], [0, 20], [5, 0]]: + status = engine.set_parameter2('Navigation', [dest, "", ""]) + print(status) + time.sleep(5) + +engine.disconnect() +print("finish") diff --git a/connection_test/scripts/pyrois/HRI_Engine_example.py b/route_guidance_ros/scripts/toplevel_engine.py similarity index 74% rename from connection_test/scripts/pyrois/HRI_Engine_example.py rename to route_guidance_ros/scripts/toplevel_engine.py index 7b6770a684a29adc44d368e15088d536b8b4f74f..c4bcf8b6c4bbc640ead64db3469b83e322582d43 100644 --- a/connection_test/scripts/pyrois/HRI_Engine_example.py +++ b/route_guidance_ros/scripts/toplevel_engine.py @@ -14,6 +14,7 @@ from __future__ import print_function import os import sys +import numpy as np from pyrois import RoIS_HRI @@ -164,12 +165,48 @@ class EventIF(RoIS_HRI.EventIF): results = ["None"] return (status, results) +from VirtualNavigation_client import VirtualNavigation_Client as VNavClient +from sub_engine_client import IF as EngineClient class IF(SystemIF, CommandIF, QueryIF, EventIF): """IF """ - pass + def __init__(self, Engine): + super().__init__(Engine) + self.command_id = 0 + self.component_clients = { + 'Navigation': VNavClient('http://localhost:8041') + } + self.engine_clients = { + 'robot1': EngineClient('http://localhost:8010'), + 'robot2': EngineClient('http://localhost:8020'), + 'robot3': EngineClient('http://localhost:8030') + } + for engine_client in self.engine_clients.values(): + engine_client.connect() + + def set_parameter(self, command_ref, parameters): + dest = np.array(parameters[0]) + robot_distance_table = {} + for name, engine in self.engine_clients.items(): + pos = engine.get_position()[3] + pos = np.array(pos) + robot_distance_table[name] = np.linalg.norm(dest - pos) + nearest_robot = min(robot_distance_table, key=robot_distance_table.get) + print("Nearest robot is '{}'".format(nearest_robot)) + + target_engine = self.engine_clients[nearest_robot] + target_engine.set_parameter(command_ref, parameters) + status = RoIS_HRI.ReturnCode_t.OK.value + self.command_id += 1 + return (status, str(self.command_id)) + def set_parameter2(self, component_ref, parameters): + print("Using VirtualNavigation") + target_component = self.component_clients[component_ref] + status = target_component.set_parameter(*parameters).value + self.command_id += 1 + return (status, str(self.command_id)) class IF_server: """IF_Server @@ -202,4 +239,4 @@ def test_engine(port): if __name__ == "__main__": - pass + test_engine(8000) diff --git a/connection_test/scripts/pyrois/HRI_Engine_client.py b/route_guidance_ros/scripts/toplevel_engine_client.py similarity index 94% rename from connection_test/scripts/pyrois/HRI_Engine_client.py rename to route_guidance_ros/scripts/toplevel_engine_client.py index 6c79f528e6ea5a60beb45ab6be5d76b62c9da60e..760af94a75ef5a2534c27aa5e6d7436f3722355b 100644 --- a/connection_test/scripts/pyrois/HRI_Engine_client.py +++ b/route_guidance_ros/scripts/toplevel_engine_client.py @@ -73,6 +73,13 @@ class CommandIF(RoIS_HRI.CommandIF): status = RoIS_HRI.ReturnCode_t(s) return (status, command_id) + #### REMOVE #### + def set_parameter2(self, component_ref, parameters): + (s, command_id) = self._proxy.set_parameter2(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) @@ -114,7 +121,6 @@ class EventIF(RoIS_HRI.EventIF): 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) diff --git a/route_guidance_ros/scripts/utilities.py b/route_guidance_ros/scripts/utilities.py new file mode 100644 index 0000000000000000000000000000000000000000..87ccf0d02a1943889e6e76cbacc25b520970536e --- /dev/null +++ b/route_guidance_ros/scripts/utilities.py @@ -0,0 +1,65 @@ +import time +from subprocess import Popen + + +class SubprocessWrapper(object): + def __init__(self, command): + self.process = Popen(command) + + def __del__(self): + self.process.kill() + + +class ComponentWrapper(SubprocessWrapper): + def __init__(self, component_name, robot_name, port): + super().__init__(["rosrun", "route_guidance_ros", component_name, str(port), robot_name]) + + +class NavigationComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("Navigation.py", robot_name, port) + + +class SysInfoComponent(ComponentWrapper): + def __init__(self, robot_name, port): + super().__init__("System_Information.py", robot_name, port) + + +class SubEngineWrapper(SubprocessWrapper): + def __init__(self, engine_port): + super().__init__(["python3", "sub_engine.py", str(engine_port)]) + + +class ToplevelEngineWrapper(SubprocessWrapper): + def __init__(self): + super().__init__(["python3", "toplevel_engine.py"]) + + +def launch_components_and_subengines(**robot_port_table): + process = [] + for robot_name, engine_port in robot_port_table.items(): + process.append(NavigationComponent(robot_name, engine_port+1)) + process.append(SysInfoComponent(robot_name, engine_port+2)) + print("Components Constructed.") + time.sleep(5) + + for engine_port in robot_port_table.values(): + process.append(SubEngineWrapper(engine_port)) + print("Sub Engine Constructed.") + time.sleep(5) + + return process + + +def setup_single_robot(): + return launch_components_and_subengines(robot1=8010) + + +def setup_multi_robot(): + process = launch_components_and_subengines(robot1=8010, robot2=8020, robot3=8030) + + process.append(ToplevelEngineWrapper()) + print("Toplevel Engine Constructed.") + time.sleep(5) + + return process