From 0bc139eaad1cb2de62042c1a4fa6a668f0ffde6f Mon Sep 17 00:00:00 2001 From: Tully Foote Date: Tue, 26 Oct 2010 21:27:25 +0000 Subject: [PATCH] to ros_comm --- core/rospy/CMakeLists.txt | 3 - core/rospy/LICENSE | 32 - core/rospy/Makefile | 1 - core/rospy/cmake/rospy.cmake | 100 -- core/rospy/epydoc.config | 12 - core/rospy/manifest.xml | 39 - core/rospy/rosdoc.yaml | 2 - core/rospy/scripts/genmsg_py.py | 92 -- core/rospy/scripts/gensrv_py.py | 111 -- core/rospy/scripts/genutil.py | 323 ------ core/rospy/src/rospy/__init__.py | 131 --- core/rospy/src/rospy/client.py | 532 --------- core/rospy/src/rospy/core.py | 516 --------- core/rospy/src/rospy/exceptions.py | 88 -- core/rospy/src/rospy/impl/__init__.py | 0 core/rospy/src/rospy/impl/init.py | 147 --- core/rospy/src/rospy/impl/masterslave.py | 523 --------- core/rospy/src/rospy/impl/paramserver.py | 108 -- core/rospy/src/rospy/impl/registration.py | 471 -------- core/rospy/src/rospy/impl/rosout.py | 116 -- core/rospy/src/rospy/impl/simtime.py | 93 -- core/rospy/src/rospy/impl/tcpros.py | 58 - core/rospy/src/rospy/impl/tcpros_base.py | 657 ----------- core/rospy/src/rospy/impl/tcpros_pubsub.py | 317 ------ core/rospy/src/rospy/impl/tcpros_service.py | 682 ----------- core/rospy/src/rospy/impl/transport.py | 190 --- core/rospy/src/rospy/impl/udpros.py | 300 ----- core/rospy/src/rospy/impl/validators.py | 51 - core/rospy/src/rospy/msg.py | 245 ---- core/rospy/src/rospy/msproxy.py | 197 ---- core/rospy/src/rospy/names.py | 328 ------ core/rospy/src/rospy/numpy_msg.py | 93 -- core/rospy/src/rospy/rostime.py | 272 ----- core/rospy/src/rospy/service.py | 147 --- core/rospy/src/rospy/topics.py | 1141 ------------------- 35 files changed, 8118 deletions(-) delete mode 100644 core/rospy/CMakeLists.txt delete mode 100644 core/rospy/LICENSE delete mode 100644 core/rospy/Makefile delete mode 100644 core/rospy/cmake/rospy.cmake delete mode 100644 core/rospy/epydoc.config delete mode 100644 core/rospy/manifest.xml delete mode 100644 core/rospy/rosdoc.yaml delete mode 100755 core/rospy/scripts/genmsg_py.py delete mode 100755 core/rospy/scripts/gensrv_py.py delete mode 100644 core/rospy/scripts/genutil.py delete mode 100644 core/rospy/src/rospy/__init__.py delete mode 100644 core/rospy/src/rospy/client.py delete mode 100644 core/rospy/src/rospy/core.py delete mode 100644 core/rospy/src/rospy/exceptions.py delete mode 100644 core/rospy/src/rospy/impl/__init__.py delete mode 100644 core/rospy/src/rospy/impl/init.py delete mode 100644 core/rospy/src/rospy/impl/masterslave.py delete mode 100644 core/rospy/src/rospy/impl/paramserver.py delete mode 100644 core/rospy/src/rospy/impl/registration.py delete mode 100644 core/rospy/src/rospy/impl/rosout.py delete mode 100644 core/rospy/src/rospy/impl/simtime.py delete mode 100644 core/rospy/src/rospy/impl/tcpros.py delete mode 100644 core/rospy/src/rospy/impl/tcpros_base.py delete mode 100644 core/rospy/src/rospy/impl/tcpros_pubsub.py delete mode 100644 core/rospy/src/rospy/impl/tcpros_service.py delete mode 100644 core/rospy/src/rospy/impl/transport.py delete mode 100644 core/rospy/src/rospy/impl/udpros.py delete mode 100644 core/rospy/src/rospy/impl/validators.py delete mode 100644 core/rospy/src/rospy/msg.py delete mode 100644 core/rospy/src/rospy/msproxy.py delete mode 100644 core/rospy/src/rospy/names.py delete mode 100644 core/rospy/src/rospy/numpy_msg.py delete mode 100644 core/rospy/src/rospy/rostime.py delete mode 100644 core/rospy/src/rospy/service.py delete mode 100644 core/rospy/src/rospy/topics.py diff --git a/core/rospy/CMakeLists.txt b/core/rospy/CMakeLists.txt deleted file mode 100644 index 394769b8..00000000 --- a/core/rospy/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -cmake_minimum_required(VERSION 2.4.6) -include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) -rosbuild_init() diff --git a/core/rospy/LICENSE b/core/rospy/LICENSE deleted file mode 100644 index 5b09ea1f..00000000 --- a/core/rospy/LICENSE +++ /dev/null @@ -1,32 +0,0 @@ -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. - diff --git a/core/rospy/Makefile b/core/rospy/Makefile deleted file mode 100644 index bbd3fc60..00000000 --- a/core/rospy/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk diff --git a/core/rospy/cmake/rospy.cmake b/core/rospy/cmake/rospy.cmake deleted file mode 100644 index 8ccb472c..00000000 --- a/core/rospy/cmake/rospy.cmake +++ /dev/null @@ -1,100 +0,0 @@ -rosbuild_find_ros_package(rospy) - -# Message-generation support. -macro(genmsg_py) - rosbuild_get_msgs(_msglist) - set(_inlist "") - set(_autogen "") - set(genmsg_py_exe ${rospy_PACKAGE_PATH}/scripts/genmsg_py.py) - - foreach(_msg ${_msglist}) - # Construct the path to the .msg file - set(_input ${PROJECT_SOURCE_DIR}/msg/${_msg}) - # Append it to a list, which we'll pass back to gensrv below - list(APPEND _inlist ${_input}) - - rosbuild_gendeps(${PROJECT_NAME} ${_msg}) - - - set(_output_py ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}/msg/_${_msg}) - string(REPLACE ".msg" ".py" _output_py ${_output_py}) - - # Add the rule to build the .py from the .msg. - add_custom_command(OUTPUT ${_output_py} - COMMAND ${genmsg_py_exe} --noinitpy ${_input} - DEPENDS ${_input} ${genmsg_py_exe} ${gendeps_exe} ${${PROJECT_NAME}_${_msg}_GENDEPS} ${ROS_MANIFEST_LIST}) - list(APPEND _autogen ${_output_py}) - endforeach(_msg) - - if(_autogen) - # Set up to create the __init__.py file that will import the .py - # files created by the above loop. It can't run until those files are - # generated, so it depends on them. - set(_output_py ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}/msg/__init__.py) - add_custom_command(OUTPUT ${_output_py} - COMMAND ${genmsg_py_exe} --initpy ${_inlist} - DEPENDS ${_autogen}) - - # A target that depends on generation of the __init__.py - add_custom_target(ROSBUILD_genmsg_py DEPENDS ${_output_py}) - # Make our target depend on rosbuild_premsgsrvgen, to allow any - # pre-msg/srv generation steps to be done first. - add_dependencies(ROSBUILD_genmsg_py rosbuild_premsgsrvgen) - # Add our target to the top-level genmsg target, which will be fired if - # the user calls genmsg() - add_dependencies(rospack_genmsg ROSBUILD_genmsg_py) - endif(_autogen) -endmacro(genmsg_py) - -# Call the macro we just defined. -genmsg_py() - -# Service-generation support. -macro(gensrv_py) - rosbuild_get_srvs(_srvlist) - set(_inlist "") - set(_autogen "") - set(gensrv_py_exe ${rospy_PACKAGE_PATH}/scripts/gensrv_py.py) - - foreach(_srv ${_srvlist}) - # Construct the path to the .srv file - set(_input ${PROJECT_SOURCE_DIR}/srv/${_srv}) - # Append it to a list, which we'll pass back to gensrv below - list(APPEND _inlist ${_input}) - - rosbuild_gendeps(${PROJECT_NAME} ${_srv}) - - - set(_output_py ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}/srv/_${_srv}) - string(REPLACE ".srv" ".py" _output_py ${_output_py}) - - # Add the rule to build the .py from the .srv - add_custom_command(OUTPUT ${_output_py} - COMMAND ${gensrv_py_exe} --noinitpy ${_input} - DEPENDS ${_input} ${gensrv_py_exe} ${gendeps_exe} ${${PROJECT_NAME}_${_srv}_GENDEPS} ${ROS_MANIFEST_LIST}) - list(APPEND _autogen ${_output_py}) - endforeach(_srv) - - if(_autogen) - # Set up to create the __init__.py file that will import the .py - # files created by the above loop. It can't run until those files are - # generated, so it depends on them. - set(_output_py ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}/srv/__init__.py) - add_custom_command(OUTPUT ${_output_py} - COMMAND ${gensrv_py_exe} --initpy ${_inlist} - DEPENDS ${_autogen}) - - # A target that depends on generation of the __init__.py - add_custom_target(ROSBUILD_gensrv_py DEPENDS ${_output_py}) - # Make our target depend on rosbuild_premsgsrvgen, to allow any - # pre-msg/srv generation steps to be done first. - add_dependencies(ROSBUILD_gensrv_py rosbuild_premsgsrvgen) - # Add our target to the top-level gensrv target, which will be fired if - # the user calls gensrv() - add_dependencies(rospack_gensrv ROSBUILD_gensrv_py) - endif(_autogen) -endmacro(gensrv_py) - -# Call the macro we just defined. -gensrv_py() - diff --git a/core/rospy/epydoc.config b/core/rospy/epydoc.config deleted file mode 100644 index b08ddd7c..00000000 --- a/core/rospy/epydoc.config +++ /dev/null @@ -1,12 +0,0 @@ -[epydoc] -name: rospy -modules: rospy, roslib.rostime -inheritance: included -url: http://ros.org/wiki/rospy -frames: no -private: no -external-api: roslib -external-api-file: roslib:doc/roslib/html/python/api-objects.txt -external-api-root: roslib:http://www.ros.org/doc/api/roslib/html/python/ -exclude: rospy.init, rospy.simtime, rospy.simtime, rospy.masterslave, rospy.msg, rospy.msnode, rospy.paramserver, rospy.registration, rospy.rosout, rospy.tcpros, rospy.tcpros_base, rospy.tcpros_pubsub, rospy.threadpool, rospy.udpros, rospy.validators, rospy.transport - diff --git a/core/rospy/manifest.xml b/core/rospy/manifest.xml deleted file mode 100644 index 539988a6..00000000 --- a/core/rospy/manifest.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - -

- rospy is a pure Python client library for ROS. The rospy client - API enables Python programmers to quickly interface with - ROS Topics, Services, - and Parameters. The - design of rospy favors implementation speed (i.e. developer time) - over runtime performance so that algorithms can be quickly - prototyped and tested within ROS. It is also ideal for - non-critical-path code, such as configuration and initialization - code. Many of the ROS tools are written in rospy to take advantage - of the type introspection capabilities. -

-

- Many of the ROS tools, such - as rostopic - and rosservice, are - built on top of rospy. -

- - - Ken Conley/kwc@willowgarage.com - BSD - - http://ros.org/wiki/rospy - - - - - - - - - - - - diff --git a/core/rospy/rosdoc.yaml b/core/rospy/rosdoc.yaml deleted file mode 100644 index 1627f9d4..00000000 --- a/core/rospy/rosdoc.yaml +++ /dev/null @@ -1,2 +0,0 @@ - - builder: epydoc - config: epydoc.config diff --git a/core/rospy/scripts/genmsg_py.py b/core/rospy/scripts/genmsg_py.py deleted file mode 100755 index aea974ff..00000000 --- a/core/rospy/scripts/genmsg_py.py +++ /dev/null @@ -1,92 +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$ - -""" -ROS message source code generation for Python - -Converts ROS .msg files in a package into Python source code implementations. -""" -import roslib; roslib.load_manifest('rospy') - -import sys -import os -import traceback - -# roslib.msgs contains the utilities for parsing .msg specifications. It is meant to have no rospy-specific knowledge -import roslib.msgs -import roslib.packages - -# genutil is a utility package the implements the package crawling -# logic of genmsg_py and gensrv_py logic -import genutil - -import roslib.genpy - -class GenmsgPackage(genutil.Generator): - """ - GenmsgPackage generates Python message code for all messages in a - package. See genutil.Generator. In order to generator code for a - single .msg file, see msg_generator. - """ - def __init__(self): - super(GenmsgPackage, self).__init__( - 'genmsg_py', 'messages', roslib.msgs.EXT, roslib.packages.MSG_DIR, roslib.genpy.MsgGenerationException) - - def generate(self, package, f, outdir): - """ - Generate python message code for a single .msg file - @param f: path to .msg file - @type f: str - @param outdir: output directory for generated code - @type outdir: str - @return: filename of generated Python code - @rtype: str - """ - verbose = True - f = os.path.abspath(f) - infile_name = os.path.basename(f) - outfile_name = self.outfile_name(outdir, infile_name) - - (name, spec) = roslib.msgs.load_from_file(f, package) - base_name = roslib.names.resource_name_base(name) - - self.write_gen(outfile_name, roslib.genpy.msg_generator(package, base_name, spec), verbose) - - roslib.msgs.register(name, spec) - return outfile_name - -if __name__ == "__main__": - roslib.msgs.set_verbose(False) - genutil.genmain(sys.argv, GenmsgPackage()) diff --git a/core/rospy/scripts/gensrv_py.py b/core/rospy/scripts/gensrv_py.py deleted file mode 100755 index 22569ad6..00000000 --- a/core/rospy/scripts/gensrv_py.py +++ /dev/null @@ -1,111 +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: gensrv_py.py 1030 2008-05-22 22:11:12Z sfkwc $ - -""" -ROS message source code generation for rospy. - -Converts ROS .srv files into Python source code implementations. -""" -import roslib; roslib.load_manifest('rospy') - -import sys, os - -import roslib.genpy -import roslib.gentools -import roslib.srvs - -import genmsg_py, genutil - -REQUEST ='Request' -RESPONSE='Response' - -class SrvGenerationException(roslib.genpy.MsgGenerationException): pass - -def srv_generator(package, name, spec): - req, resp = ["%s%s"%(name, suff) for suff in [REQUEST, RESPONSE]] - - fulltype = '%s%s%s'%(package, roslib.srvs.SEP, name) - - gendeps_dict = roslib.gentools.get_dependencies(spec, package) - md5 = roslib.gentools.compute_md5(gendeps_dict) - - yield "class %s(roslib.message.ServiceDefinition):"%name - yield " _type = '%s'"%fulltype - yield " _md5sum = '%s'"%md5 - yield " _request_class = %s"%req - yield " _response_class = %s"%resp - -class SrvGenerator(genutil.Generator): - def __init__(self): - super(SrvGenerator, self).__init__('gensrv_py', 'services', roslib.srvs.EXT, 'srv', SrvGenerationException) - - def generate(self, package, f, outdir): - verbose = True - f = os.path.abspath(f) - infile_name = os.path.basename(f) - if not os.path.exists(outdir): - os.makedirs(outdir) - elif not os.path.isdir(outdir): - raise SrvGenerationException("Cannot write to %s: file in the way"%outdir) - - prefix = infile_name[:-len(roslib.srvs.EXT)] - # generate message files for request/response - name, spec = roslib.srvs.load_from_file(f, package) - base_name = roslib.names.resource_name_base(name) - - outfile = self.outfile_name(outdir, f) - f = open(outfile, 'w') - if verbose: - print "... generating %s"%outfile - try: - for mspec, suffix in ((spec.request, REQUEST), (spec.response, RESPONSE)): - #outfile = os.path.join(outdir, prefix+suffix+".py") - #gen = roslib.genpy.msg_generator(package, name+suffix, mspec) - #self.write_gen(outfile, gen, roslib.srvs.is_verbose()) - for l in roslib.genpy.msg_generator(package, base_name+suffix, mspec): - f.write(l+'\n') - - # generate service file - #outfile = os.path.join(outdir, prefix+".py") - #self.write_gen(outfile, srv_generator(package, name, spec), verbose) - for l in srv_generator(package, base_name, spec): - f.write(l+'\n') - finally: - f.close() - return outfile - -if __name__ == "__main__": - roslib.srvs.set_verbose(False) - genutil.genmain(sys.argv, SrvGenerator()) diff --git a/core/rospy/scripts/genutil.py b/core/rospy/scripts/genutil.py deleted file mode 100644 index 8a550ea6..00000000 --- a/core/rospy/scripts/genutil.py +++ /dev/null @@ -1,323 +0,0 @@ -# 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: genmsg_py.py 1590 2008-07-22 18:30:01Z sfkwc $ - -## Common generation tools for Python ROS message and service generators - -import os -import errno # for smart handling of exceptions for os.makedirs() -import sys -import traceback -import glob - -import roslib.msgs -import roslib.packages -import roslib.resources -from roslib.genpy import MsgGenerationException - -class Generator(object): - - ## @param name str: name of resource types - ## @param ext str: file extension of resources (e.g. '.msg') - ## @param subdir str: directory sub-path of resources (e.g. 'msg') - ## @param exception class: exception class to raise if an error occurs - def __init__(self, name, what, ext, subdir, exception): - self.name = name - self.what = what - self.subdir = subdir - self.ext = ext - self.exception = exception - - ## @return [str]: list of (message/service) types in specified package - def list_types(self, package): - ext = self.ext - types = roslib.resources.list_package_resources( - package, False, self.subdir, - lambda x: os.path.isfile(x) and x.endswith(ext)) - return [x[:-len(ext)] for x in types] - - ## Convert resource filename to ROS resource name - ## @param filename str: path to .msg/.srv file - ## @return str: name of ROS resource - def resource_name(self, filename): - return os.path.basename(filename)[:-len(self.ext)] - - ## @param type_name str: Name of message type sans package, - ## e.g. 'String' - ## @return str: name of python module for auto-generated code - def _module_name(self, type_name): - return "_"+type_name - - ## @param outdir str: path to directory that files are generated to - ## @return str: output file path based on input file name and output directory - def outfile_name(self, outdir, infile_name): - # Use leading _ so that module name does not collide with message name. It also - # makes it more clear that the .py file should not be imported directly - return os.path.join(outdir, self._module_name(self.resource_name(infile_name))+".py") - - ## @param base_dir str: base directory of package - ## @return str: out directory for generated files. For messages, this is - ## package 'mypackage.msg', stored in the src directory (.srv for services). - # - this is different from C++ and Ruby, mainly because Python cannot unify packages - # across multiple directories. The msg subpackage is because we want to cleanly - # denote auto-generated files. - def outdir(self, basedir): - "compute the directory that the .py files are output to" - outdir = os.path.join(basedir, roslib.packages.SRC_DIR, os.path.basename(basedir), self.subdir) - if not os.path.exists(outdir): - try: - os.makedirs(outdir) - except Exception, e: - # It's not a problem if the directory already exists, - # because this can happen during a parallel build - if e.errno != errno.EEXIST: - raise e - - elif not os.path.isdir(outdir): - raise self.exception("Cannot write to %s: file in the way"%outdir) - return outdir - - def generate(self, package, f, outdir): - raise Exception('subclass must override') - - ## reindex files as a dictionary keyed by package - def generate_package_files(self, package_files, files, ext): - files = filter(lambda f: f.endswith(ext), files) - retcode = 0 - for f in files: - try: - package_dir, package = roslib.packages.get_dir_pkg(f) - outdir = self.outdir(package_dir) - if not package: - raise self.exception("Cannot locate package for %s. Is ROS_ROOT set?"%f) - outfile_name = self.outfile_name(outdir, f) - if not package in package_files: - package_files[package] = [] - package_files[package].append(f) - except Exception, e: - print "\nERROR[%s]: Unable to load %s file '%s': %s\n"%(self.name, self.ext, f, e) - retcode = 1 #flag error - return retcode - - def write_modules(self, package_files): - for package, pfiles in package_files.iteritems(): - mfiles = [self.resource_name(f) for f in pfiles] - package_dir = roslib.packages.get_pkg_dir(package, True) - outdir = self.outdir(package_dir) - #TODO: also check against MSG/SRV dir to make sure it really is a generated file - # get a list of all the python files in the generated directory - # so we can import them into __init__.py. We intersect that list with the - # list of the .msg files so we can catch deletions without having to 'make clean' - good_types = set([f[1:-3] for f in os.listdir(outdir) - if f.endswith('.py') and f != '__init__.py']) - types = set(self.list_types(package)) - generated_modules = [self._module_name(f) for f in good_types.intersection(types)] - if package_dir is None: - continue #already printed error message about - - self.write_module(package_dir, package, generated_modules) - return 0 - - ## @param base_dir str: path to package - ## @param package str: name of package to write module for - ## @param generated_modules [str]: list of generated message modules, - ## i.e. the names of the .py files that were generated for each - ## .msg file. - def write_module(self, basedir, package, generated_modules): - """create a module file to mark directory for python""" - dir = self.outdir(basedir) - if not os.path.exists(dir): - os.makedirs(dir) - elif not os.path.isdir(dir): - raise self.exception("file preventing the creating of module directory: %s"%dir) - p = os.path.join(dir, '__init__.py') - if roslib.msgs.is_verbose(): - print "... creating module file", p - f = open(p, 'w') - try: - #this causes more problems than anticipated -- for pure python - #packages it works fine, but in C++ packages doxygen seems to prefer python first. - #f.write('## \mainpage\n') #doxygen - #f.write('# \htmlinclude manifest.html\n') - for mod in generated_modules: - f.write('from %s import *\n'%mod) - finally: - f.close() - - parentInit = os.path.dirname(dir) - p = os.path.join(parentInit, '__init__.py') - if not os.path.exists(p): - #touch __init__.py in the parent package - print "... also creating module file %s"%p - f = open(p, 'w') - f.close() - - def generate_package(self, package, pfiles): - if not roslib.names.is_legal_resource_base_name(package): - print "\nERROR[%s]: package name '%s' is illegal and cannot be used in message generation.\nPlease see http://ros.org/wiki/Names"%(self.name, package) - return 1 # flag error - - package_dir = roslib.packages.get_pkg_dir(package, True) - if package_dir is None: - print "\nERROR[%s]: Unable to locate package '%s'\n"%(self.name, package) - return 1 #flag error - - # package/src/package/msg for messages, packages/src/package/srv for services - outdir = self.outdir(package_dir) - try: - # TODO: this can result in packages getting dependencies - # that they shouldn't. To implement this correctly will - # require an overhaul of the message generator - # infrastructure. - roslib.msgs.load_package_dependencies(package, load_recursive=True) - except Exception, e: - #traceback.print_exc() - print "\nERROR[%s]: Unable to load package dependencies for %s: %s\n"%(self.name, package, e) - return 1 #flag error - try: - roslib.msgs.load_package(package) - except Exception, e: - print "\nERROR[%s]: Unable to load package %s: %s\n"%(self.name, package, e) - return 1 #flag error - - #print "[%s]: [%s] generating %s for the following messages: %s"%(self.name, self.what, package, pfiles) - retcode = 0 - for f in pfiles: - try: - outfile = self.generate(package, f, outdir) #actual generation - except Exception, e: - if not isinstance(e, MsgGenerationException) and not isinstance(e, roslib.msgs.MsgSpecException): - traceback.print_exc() - print "\nERROR[%s]: Unable to generate %s for package '%s': while processing '%s': %s\n"%(self.name, self.what, package, f, e) - retcode = 1 #flag error - print "%s: python messages for '%s' ==> %s"%(self.name, package, outdir) - return retcode - - def generate_all_by_package(self, package_files): - """ - @return: return code - @rtype: int - """ - retcode = 0 - for package, pfiles in package_files.iteritems(): - retcode = self.generate_package(package, pfiles) or retcode - return retcode - - def generate_initpy(self, files): - """ - Generate __init__.py file for each package in in the msg/srv file list - and have the __init__.py file import the required symbols from - the generated version of the files. - @param files: list of msg/srv files - @type files: [str] - @return: return code - @rtype: int - """ - - package_files = {} - # pass 1: collect list of files for each package - retcode = self.generate_package_files(package_files, files, self.ext) - - # pass 2: write the __init__.py file for the module - retcode = retcode or self.write_modules(package_files) - - def generate_messages(self, files, no_gen_initpy): - """ - @param no_gen_initpy: if True, don't generate the __init__.py - file. This option is for backwards API compatibility. - @type no_gen_initpy: bool - @return: return code - @rtype: int - """ - package_files = {} - # pass 1: collect list of files for each package - retcode = self.generate_package_files(package_files, files, self.ext) - print "[%s]: generating %s for the following packages: %s"%(self.name, self.what, package_files.keys()) - - # pass 2: roslib.msgs.load_package(), generate messages - retcode = retcode or self.generate_all_by_package(package_files) - - # backwards compat - if not no_gen_initpy: - retcode = retcode or self.write_modules(package_files) - - return retcode - - def write_gen(self, outfile, gen, verbose): - if verbose: - print "... generating %s"%outfile - f = open(outfile, 'w') - try: - for l in gen: - f.write(l+'\n') - finally: - f.close() - -def usage(progname): - print "%(progname)s file(s)"%vars() - -def get_files(argv, usage_fn, ext): - if not argv[1:]: - usage_fn(argv[0]) - files = [] - for arg in argv[1:]: - if not arg == '--initpy': - files.extend(glob.glob(arg)) - return files - -def genmain(argv, gen, usage_fn=usage): - try: - gen_initpy = '--initpy' in argv - no_gen_initpy = '--noinitpy' in argv - - if gen_initpy: - # #1827 - packages = [p for p in argv[1:] if not p == '--initpy'] - retcode = gen.generate_initpy(packages) - else: - files = get_files(argv, usage_fn, gen.ext) - if not files: - print "No matching files found" - return - retcode = gen.generate_messages(files, no_gen_initpy) - except roslib.msgs.MsgSpecException, e: - print >> sys.stderr, "ERROR: ", e - retcode = 1 - except MsgGenerationException, e: - print sys.stderr, "ERROR: ",e - retcode = 2 - except Exception, e: - traceback.print_exc() - print "ERROR: ",e - retcode = 3 - sys.exit(retcode or 0) diff --git a/core/rospy/src/rospy/__init__.py b/core/rospy/src/rospy/__init__.py deleted file mode 100644 index 9847ece4..00000000 --- a/core/rospy/src/rospy/__init__.py +++ /dev/null @@ -1,131 +0,0 @@ -# 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. -# -# Copyright (c) 2008, Willow Garage, Inc. -# Revision $Id$ - -""" -ROS client library for Python. -See U{http://ros.org/wiki/rospy} -@author: Ken Conley (kwc) -""" - -from roslib.rosenv import ROS_ROOT, ROS_MASTER_URI, ROS_HOSTNAME, ROS_NAMESPACE, ROS_PACKAGE_PATH, ROS_LOG_DIR - -# import symbols into rospy namespace -# NOTE: there are much better ways to configure python module -# dictionaries, but the rospy codebase isn't quite in shape for that -# yet - -from roslib.msg import Header - -from .client import spin, myargv, init_node, \ - get_published_topics, \ - wait_for_message, \ - get_master, \ - on_shutdown, \ - get_param, get_param_names, set_param, delete_param, has_param, search_param,\ - sleep, Rate,\ - DEBUG, INFO, WARN, ERROR, FATAL -from .core import is_shutdown, signal_shutdown, \ - get_node_uri, get_ros_root, \ - logdebug, logwarn, loginfo, logout, logerr, logfatal, \ - parse_rosrpc_uri -from .exceptions import * -from .msg import AnyMsg -from .msproxy import MasterProxy -from .names import get_name, get_caller_id, get_namespace, resolve_name, remap_name -from .rostime import Time, Duration, get_rostime, get_time -from .service import ServiceException, ServiceDefinition - -# - use tcp ros implementation of services -from .impl.tcpros_service import Service, ServiceProxy, wait_for_service -from .topics import Message, SubscribeListener, Publisher, Subscriber - -## \defgroup validators Validators -## \defgroup clientapi Client API - -# initialize default loggers, this has to happen independent of a node being created -from .impl import init -init.init_log_handlers() - -__all__ = [ - 'Header', - 'spin', - 'myargv', - 'init_node', - 'get_master', - 'get_published_topics', - 'wait_for_service', - 'on_shutdown', - 'get_param', - 'get_param_names', - 'set_param', - 'delete_param', - 'has_param', - 'search_param', - 'sleep', - 'Rate', - 'DEBUG', - 'INFO', - 'WARN', - 'ERROR', - 'FATAL' - 'is_shutdown', - 'signal_shutdown', - 'get_node_uri', - 'get_ros_root', - 'logdebug', - 'logwarn', 'loginfo', - 'logout', 'logerr', 'logfatal', - 'parse_rosrpc_uri', - 'MasterProxy', - 'NodeProxy', - 'ROSException', - 'ROSSerializationException', - 'ROSInitException', - 'ROSInterruptException', - 'ROSInternalException', - 'TransportException', - 'TransportTerminated', - 'TransportInitError', - 'AnyMsg', 'Message', - 'get_name', - 'get_caller_id', - 'get_namespace', - 'resolve_name', - 'remap_name', - 'Time', 'Duration', 'get_rostime', 'get_time', - 'ServiceException', 'ServiceDefinition', - 'Service', 'ServiceProxy', - 'SubscribeListener', 'Publisher', 'Subscriber', - ] diff --git a/core/rospy/src/rospy/client.py b/core/rospy/src/rospy/client.py deleted file mode 100644 index 235e29c5..00000000 --- a/core/rospy/src/rospy/client.py +++ /dev/null @@ -1,532 +0,0 @@ -# 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$ - -""" -Additional ROS client API methods. -""" - -import logging -import os -import socket -import struct -import sys -import time -import random - -import roslib -import roslib.msg -import roslib.names -import roslib.network - -import rospy.core -from rospy.core import logwarn, loginfo, logerr, logdebug -import rospy.exceptions -import rospy.names -import rospy.rostime - -import rospy.impl.init -import rospy.impl.rosout -import rospy.impl.simtime - -TIMEOUT_READY = 15.0 #seconds - -# log level constants -DEBUG = roslib.msg.Log.DEBUG -INFO = roslib.msg.Log.INFO -WARN = roslib.msg.Log.WARN -ERROR = roslib.msg.Log.ERROR -FATAL = roslib.msg.Log.FATAL - -_rospy_to_logging_levels = { - DEBUG: logging.DEBUG, - INFO: logging.INFO, - WARN: logging.WARNING, - ERROR: logging.ERROR, - FATAL: logging.CRITICAL, - } - - -def on_shutdown(h): - """ - Register function to be called on shutdown. This function will be - called before Node begins teardown. - @param h: Function with zero args to be called on shutdown. - @type h: fn() - """ - rospy.core.add_client_shutdown_hook(h) - -def spin(): - """ - Blocks until ROS node is shutdown. Yields activity to other threads. - @raise ROSInitException: if node is not in a properly initialized state - """ - - if not rospy.core.is_initialized(): - raise rospy.exceptions.ROSInitException("client code must call rospy.init_node() first") - logdebug("node[%s, %s] entering spin(), pid[%s]", rospy.core.get_caller_id(), rospy.core.get_node_uri(), os.getpid()) - try: - while not rospy.core.is_shutdown(): - time.sleep(0.5) - except KeyboardInterrupt: - logdebug("keyboard interrupt, shutting down") - rospy.core.signal_shutdown('keyboard interrupt') - -def myargv(argv=sys.argv): - """ - Helper function for using sys.argv is ROS client libraries. - @return: copy of sys.argv with ROS remapping arguments removed - @rtype: [str] - """ - import roslib.scriptutil - return roslib.scriptutil.myargv(argv=argv) - -def _init_node_params(argv, node_name): - """ - Uploads private params to the parameter server. Private params are specified - via command-line remappings. - """ - - # #1027: load in param name mappings - import roslib.params - params = roslib.params.load_command_line_node_params(argv) - for param_name, param_value in params.iteritems(): - logdebug("setting param %s to %s"%(param_name, param_value)) - set_param(roslib.names.PRIV_NAME + param_name, param_value) - -_init_node_args = None - -def init_node(name, argv=None, anonymous=False, log_level=INFO, disable_rostime=False, disable_rosout=False, disable_signals=False): - """ - Register client node with the master under the specified name. - This MUST be called from the main Python thread unless - disable_signals is set to True. Duplicate calls to init_node are - only allowed if the arguments are identical as the side-effects of - this method are not reversible. - - @param name: Node's name. This parameter must be a base name, - meaning that it cannot contain namespaces (i.e. '/') - @type name: str - - @param argv: Command line arguments to this program, including - remapping arguments (default: sys.argv). If you provide argv - to init_node(), any previously created rospy data structure - (Publisher, Subscriber, Service) will have invalid - mappings. It is important that you call init_node() first if - you wish to provide your own argv. - @type argv: [str] - - @param anonymous: if True, a name will be auto-generated for the - node using name as the base. This is useful when you wish to - have multiple instances of the same node and don't care about - their actual names (e.g. tools, guis). name will be used as - the stem of the auto-generated name. NOTE: you cannot remap - the name of an anonymous node. - @type anonymous: bool - - @param log_level: log level for sending message to /rosout and log - file, which is INFO by default. For convenience, you may use - rospy.DEBUG, rospy.INFO, rospy.ERROR, rospy.WARN, rospy.FATAL - @type log_level: int - - @param disable_signals: If True, rospy will not register its own - signal handlers. You must set this flag if (a) you are unable - to call init_node from the main thread and/or you are using - rospy in an environment where you need to control your own - signal handling (e.g. WX). If you set this to True, you should - call rospy.signal_shutdown(reason) to initiate clean shutdown. - - NOTE: disable_signals is overridden to True if - roslib.is_interactive() is True. - - @type disable_signals: bool - - @param disable_rostime: for rostests only, suppresses - automatic subscription to rostime - @type disable_rostime: bool - - @param disable_rosout: suppress auto-publication of rosout - @type disable_rostime: bool - - @raise ROSInitException: if initialization/registration fails - @raise ValueError: if parameters are invalid (e.g. name contains a namespace or is otherwise illegal) - """ - if argv is None: - argv = sys.argv - else: - # reload the mapping table. Any previously created rospy data - # structure does *not* reinitialize based on the new mappings. - rospy.names.reload_mappings(argv) - - # this test can be eliminated once we change from warning to error in the next check - if roslib.names.SEP in name: - raise ValueError("namespaces are not allowed in node names") - if not roslib.names.is_legal_base_name(name): - import warnings - warnings.warn("'%s' is not a legal ROS base name. This may cause problems with other ROS tools"%name, stacklevel=2) - - global _init_node_args - - # #972: allow duplicate init_node args if calls are identical - # NOTE: we don't bother checking for node name aliases (e.g. 'foo' == '/foo'). - if _init_node_args: - if _init_node_args != (name, argv, anonymous, log_level, disable_rostime, disable_signals): - raise rospy.exceptions.ROSException("rospy.init_node() has already been called with different arguments: "+str(_init_node_args)) - else: - return #already initialized - - # for scripting environments, we don't want to use the ROS signal - # handlers - disable_signals = disable_signals or roslib.is_interactive() - _init_node_args = (name, argv, anonymous, log_level, disable_rostime, disable_signals) - - if not disable_signals: - # NOTE: register_signals must be called from main thread - rospy.core.register_signals() # add handlers for SIGINT/etc... - else: - logdebug("signal handlers for rospy disabled") - - # check for name override - mappings = rospy.names.get_mappings() - if '__name' in mappings: - # use roslib version of resolve_name to avoid remapping - name = roslib.names.resolve_name(mappings['__name'], rospy.core.get_caller_id()) - if anonymous: - logdebug("[%s] WARNING: due to __name setting, anonymous setting is being changed to false"%name) - anonymous = False - - if anonymous: - # not as good as a uuid/guid, but more readable. can't include - # hostname as that is not guaranteed to be a legal ROS name - name = "%s_%s_%s"%(name, os.getpid(), int(time.time()*1000)) - - resolved_node_name = rospy.names.resolve_name(name) - rospy.core.configure_logging(resolved_node_name, level=_rospy_to_logging_levels[log_level]) - # #1810 - rospy.names.initialize_mappings(resolved_node_name) - - logger = logging.getLogger("rospy.client") - logger.info("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid()) - - # node initialization blocks until registration with master - node = rospy.impl.init.start_node(os.environ, resolved_node_name) - rospy.core.set_node_uri(node.uri) - rospy.core.add_shutdown_hook(node.shutdown) - - if rospy.core.is_shutdown(): - logger.warn("aborting node initialization as shutdown has been triggered") - raise rospy.exceptions.ROSInitException("init_node interrupted before it could complete") - - # upload private params (set via command-line) to parameter server - _init_node_params(argv, name) - - rospy.core.set_initialized(True) - - rospy.impl.rosout.load_rosout_handlers(log_level) - if not disable_rosout: - rospy.impl.rosout.init_rosout() - logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid()) - if not disable_rostime: - if not rospy.impl.simtime.init_simtime(): - raise rospy.exceptions.ROSInitException("Failed to initialize time. Please check logs for additional details") - else: - rospy.rostime.set_rostime_initialized(True) - -#_master_proxy is a MasterProxy wrapper -_master_proxy = None - -def get_master(env=os.environ): - """ - Get a remote handle to the ROS Master. - This method can be called independent of running a ROS node, - though the ROS_MASTER_URI must be declared in the environment. - - @return: ROS Master remote object - @rtype: L{rospy.MasterProxy} - @raise Exception: if server cannot be located or system cannot be - initialized - """ - global _master_proxy - if _master_proxy is not None: - return _master_proxy - import roslib.rosenv - _master_proxy = rospy.msproxy.MasterProxy(roslib.rosenv.get_master_uri()) - return _master_proxy - -######################################################### -# Topic helpers - -def get_published_topics(namespace='/'): - """ - Retrieve list of topics that the master is reporting as being published. - - @return: List of topic names and types: [[topic1, type1]...[topicN, typeN]] - @rtype: [[str, str]] - """ - code, msg, val = get_master().getPublishedTopics(namespace) - if code != 1: - raise rospy.exceptions.ROSException("unable to get published topics: %s"%msg) - return val - -class _WFM(object): - def __init__(self): - self.msg = None - def cb(self, msg): - if self.msg is None: - self.msg = msg - -def wait_for_message(topic, topic_type, timeout=None): - """ - Receive one message from topic. - - This will create a new subscription to the topic, receive one message, then unsubscribe. - - @param topic: name of topic - @type topic: str - @param topic_type: topic type - @type topic_type: L{rospy.Message} class - @param timeout: timeout time in seconds - @type timeout: double - @return: Message - @rtype: L{rospy.Message} - @raise ROSException: if specified timeout is exceeded - @raise ROSInterruptException: if shutdown interrupts wait - """ - wfm = _WFM() - s = None - try: - s = rospy.topics.Subscriber(topic, topic_type, wfm.cb) - if timeout is not None: - timeout_t = time.time() + timeout - while not rospy.core.is_shutdown() and wfm.msg is None: - time.sleep(0.01) - if time.time() >= timeout_t: - raise rospy.exceptions.ROSException("timeout exceeded while waiting for message on topic %s"%topic) - - else: - while not rospy.core.is_shutdown() and wfm.msg is None: - time.sleep(0.01) - finally: - if s is not None: - s.unregister() - if rospy.core.is_shutdown(): - raise rospy.exceptions.ROSInterruptException("rospy shutdown") - return wfm.msg - - -######################################################### -# Param Server Access - -_param_server = None -def _init_param_server(): - """ - Initialize parameter server singleton - """ - global _param_server - if _param_server is None: - _param_server = get_master() #in the future param server will be a service - -# class and singleton to distinguish whether or not user has passed us a default value -class _Unspecified(object): pass -_unspecified = _Unspecified() - -def get_param(param_name, default=_unspecified): - """ - Retrieve a parameter from the param server - @param default: (optional) default value to return if key is not set - @type default: any - @return: parameter value - @rtype: XmlRpcLegalValue - @raise ROSException: if parameter server reports an error - @raise KeyError: if value not set and default is not given - """ - try: - _init_param_server() - return _param_server[param_name] #MasterProxy does all the magic for us - except KeyError: - if default != _unspecified: - return default - else: - raise - -def get_param_names(): - """ - Retrieve list of parameter names. - @return: parameter names - @rtype: [str] - @raise ROSException: if parameter server reports an error - """ - _init_param_server() - code, msg, val = _param_server.getParamNames() #MasterProxy does all the magic for us - if code != 1: - raise rospy.exceptions.ROSException("Unable to retrieve parameter names: %s"%msg) - else: - return val - -def set_param(param_name, param_value): - """ - Set a parameter on the param server - @param param_name: parameter name - @type param_name: str - @param param_value: parameter value - @type param_value: XmlRpcLegalValue - @raise ROSException: if parameter server reports an error - """ - # #2202 - if not roslib.names.is_legal_name(param_name): - import warnings - warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%param_name, stacklevel=2) - - _init_param_server() - _param_server[param_name] = param_value #MasterProxy does all the magic for us - -def search_param(param_name): - """ - Search for a parameter on the param server - @param param_name: parameter name - @type param_name: str - @return: key of matching parameter or None if no matching parameter. - @rtype: str - @raise ROSException: if parameter server reports an error - """ - _init_param_server() - return _param_server.search_param(param_name) - -def delete_param(param_name): - """ - Delete a parameter on the param server - @param param_name: parameter name - @type param_name: str - @raise KeyError: if parameter is not set - @raise ROSException: if parameter server reports an error - """ - _init_param_server() - del _param_server[param_name] #MasterProxy does all the magic for us - -def has_param(param_name): - """ - Test if parameter exists on the param server - @param param_name: parameter name - @type param_name: str - @raise ROSException: if parameter server reports an error - """ - _init_param_server() - return param_name in _param_server #MasterProxy does all the magic for us - -################################################################################ -# Time helpers - -# these cannot go into rostime due to circular deps - - -class Rate(object): - """ - Convenience class for sleeping in a loop at a specified rate - """ - - def __init__(self, hz): - """ - Constructor. - @param hz: hz rate to determine sleeping - @type hz: int - """ - # #1403 - self.last_time = rospy.rostime.get_rostime() - self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz)) - - def sleep(self): - """ - Attempt sleep at the specified rate. sleep() takes into - account the time elapsed since the last successful - sleep(). - - @raise ROSInterruptException: if ROS time is set backwards or if - ROS shutdown occurs before sleep completes - """ - curr_time = rospy.rostime.get_rostime() - # detect time jumping backwards - if self.last_time > curr_time: - self.last_time = curr_time - - # calculate sleep interval - elapsed = curr_time - self.last_time - sleep(self.sleep_dur - elapsed) - self.last_time = self.last_time + self.sleep_dur - - # detect time jumping forwards, as well as loops that are - # inherently too slow - if curr_time - self.last_time > self.sleep_dur * 2: - self.last_time = curr_time - -# TODO: may want more specific exceptions for sleep -def sleep(duration): - """ - sleep for the specified duration in ROS time. If duration - is negative, sleep immediately returns. - - @param duration: seconds (or rospy.Duration) to sleep - @type duration: float or Duration - @raise ROSInterruptException: if ROS time is set backwards or if - ROS shutdown occurs before sleep completes - """ - if rospy.rostime.is_wallclock(): - if isinstance(duration, roslib.rostime.Duration): - duration = duration.to_sec() - if duration < 0: - return - else: - time.sleep(duration) - else: - initial_rostime = rospy.rostime.get_rostime() - if not isinstance(duration, roslib.rostime.Duration): - duration = rospy.rostime.Duration.from_sec(duration) - sleep_t = initial_rostime + duration - - rostime_cond = rospy.rostime.get_rostime_cond() - - # break loop if sleep_t is reached, time moves backwards, or - # node is shutdown - while rospy.rostime.get_rostime() < sleep_t and \ - rospy.rostime.get_rostime() >= initial_rostime and \ - not rospy.core.is_shutdown(): - try: - rostime_cond.acquire() - rostime_cond.wait(0.5) - finally: - rostime_cond.release() - - if rospy.rostime.get_rostime() < initial_rostime: - raise rospy.exceptions.ROSInterruptException("ROS time moved backwards") - if rospy.core.is_shutdown(): - raise rospy.exceptions.ROSInterruptException("ROS shutdown request") - diff --git a/core/rospy/src/rospy/core.py b/core/rospy/src/rospy/core.py deleted file mode 100644 index fac7fd23..00000000 --- a/core/rospy/src/rospy/core.py +++ /dev/null @@ -1,516 +0,0 @@ -# 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$ - -"""rospy internal core implementation library""" - -from __future__ import with_statement - -import atexit -import logging -import os -import signal -import sys -import threading -import time -import traceback -import types -import urlparse -import xmlrpclib - -import roslib.msg -import roslib.rosenv -import roslib.roslogging - -import rospy.exceptions - -from rospy.names import * -from rospy.impl.validators import ParameterInvalid - -_logger = logging.getLogger("rospy.core") - -# number of seconds to wait to join on threads. network issue can -# cause joins to be not terminate gracefully, and it's better to -# teardown dirty than to hang -_TIMEOUT_SHUTDOWN_JOIN = 5. - -import warnings -def deprecated(func): - """This is a decorator which can be used to mark functions - as deprecated. It will result in a warning being emmitted - when the function is used.""" - def newFunc(*args, **kwargs): - warnings.warn("Call to deprecated function %s." % func.__name__, - category=DeprecationWarning, stacklevel=2) - return func(*args, **kwargs) - newFunc.__name__ = func.__name__ - newFunc.__doc__ = func.__doc__ - newFunc.__dict__.update(func.__dict__) - return newFunc - -######################################################### -# ROSRPC - -ROSRPC = "rosrpc://" - -def parse_rosrpc_uri(uri): - """ - utility function for parsing ROS-RPC URIs - @param uri: ROSRPC URI - @type uri: str - @return: address, port - @rtype: (str, int) - @raise ParameterInvalid: if uri is not a valid ROSRPC URI - """ - if uri.startswith(ROSRPC): - dest_addr = uri[len(ROSRPC):] - else: - raise ParameterInvalid("Invalid protocol for ROS service URL: %s"%uri) - try: - if '/' in dest_addr: - dest_addr = dest_addr[:dest_addr.find('/')] - dest_addr, dest_port = dest_addr.split(':') - dest_port = int(dest_port) - except: - raise ParameterInvalid("ROS service URL is invalid: %s"%uri) - return dest_addr, dest_port - -######################################################### - -# rospy logger -_rospy_logger = logging.getLogger("rospy.internal") - -_logdebug_handlers = [] -_loginfo_handlers = [] -_logwarn_handlers = [] -_logerr_handlers = [] -_logfatal_handlers = [] - -# we keep a separate, non-rosout log file to contain stack traces and -# other sorts of information that scare users but are essential for -# debugging - -def rospydebug(msg, *args): - """Internal rospy client library debug logging""" - _rospy_logger.debug(msg, *args) -def rospyerr(msg, *args): - """Internal rospy client library error logging""" - _rospy_logger.error(msg, *args) -def rospywarn(msg, *args): - """Internal rospy client library warn logging""" - _rospy_logger.warn(msg, *args) - -def add_log_handler(level, h): - """ - Add handler for specified level - @param level: log level (use constants from roslib.msg.Log) - @type level: int - @param h: log message handler - @type h: fn - @raise ROSInternalException: if level is invalid - """ - if level == roslib.msg.Log.DEBUG: - _logdebug_handlers.append(h) - elif level == roslib.msg.Log.INFO: - _loginfo_handlers.append(h) - elif level == roslib.msg.Log.WARN: - _logwarn_handlers.append(h) - elif level == roslib.msg.Log.ERROR: - _logerr_handlers.append(h) - elif level == roslib.msg.Log.FATAL: - _logfatal_handlers.append(h) - else: - raise rospy.exceptions.ROSInternalException("invalid log level: %s"%level) - -def logdebug(msg, *args): - """ - Log a debug message to the /rosout topic - @param msg: message to log, may include formatting arguments - @type msg: str - @param args: format-string arguments, if necessary - """ - if args: - msg = msg%args - for h in _logdebug_handlers: - h(msg) - -def logwarn(msg, *args): - """ - Log a warning message to the /rosout topic - @param msg: message to log, may include formatting arguments - @type msg: str - @param args: format-string arguments, if necessary - """ - if args: - msg = msg%args - for h in _logwarn_handlers: - h(msg) - -def loginfo(msg, *args): - """ - Log an info message to the /rosout topic - @param msg: message to log, may include formatting arguments - @type msg: str - @param args: format-string arguments, if necessary - """ - if args: - msg = msg%args - for h in _loginfo_handlers: - h(msg) -logout = loginfo # alias deprecated name - -def logerr(msg, *args): - """ - Log an error message to the /rosout topic - @param msg: message to log, may include formatting arguments - @type msg: str - @param args: format-string arguments, if necessary - """ - if args: - msg = msg%args - for h in _logerr_handlers: - h(msg) -logerror = logerr # alias logerr - -def logfatal(msg, *args): - """ - Log an error message to the /rosout topic - @param msg: message to log, may include formatting arguments - @type msg: str - @param args: format-string arguments, if necessary - """ - if args: - msg = msg%args - for h in _logfatal_handlers: - h(msg) - -######################################################### -# CONSTANTS - -MASTER_NAME = "master" #master is a reserved node name for the central master - -def get_ros_root(required=False, env=None): - """ - Get the value of ROS_ROOT. - @param env: override environment dictionary - @type env: dict - @param required: if True, fails with ROSException - @return: Value of ROS_ROOT environment - @rtype: str - @raise ROSException: if require is True and ROS_ROOT is not set - """ - if env is None: - env = os.environ - ros_root = env.get(roslib.rosenv.ROS_ROOT, None) - if required and not ros_root: - raise rospy.exceptions.ROSException('%s is not set'%roslib.rosenv.ROS_ROOT) - return ros_root - - -######################################################### -# API - -_uri = None -def get_node_uri(): - """ - Get this Node's URI. - @return: this Node's XMLRPC URI - @rtype: str - """ - return _uri - -def set_node_uri(uri): - """set the URI of the local node. - This is an internal API method, it does not actually affect the XMLRPC URI of the Node.""" - global _uri - _uri = uri - -######################################################### -# Logging - -_log_filename = None -def configure_logging(node_name, level=logging.INFO): - """ - Setup filesystem logging for this node - @param node_name: Node's name - @type node_name str - @param level: (optional) Python logging level (INFO, DEBUG, etc...). (Default: logging.INFO) - @type level: int - """ - global _log_filename - - # #988 __log command-line remapping argument - mappings = get_mappings() - if '__log' in get_mappings(): - logfilename_remap = mappings['__log'] - filename = os.path.abspath(logfilename_remap) - else: - # fix filesystem-unsafe chars - filename = node_name.replace('/', '_') + '.log' - if filename[0] == '_': - filename = filename[1:] - if not filename: - raise rospy.exceptions.ROSException('invalid configure_logging parameter: %s'%node_name) - _log_filename = roslib.roslogging.configure_logging('rospy', level, filename=filename) - -class NullHandler(logging.Handler): - def emit(self, record): - pass - -# keep logging happy until we have the node name to configure with -logging.getLogger('rospy').addHandler(NullHandler()) - - -######################################################### -# Init/Shutdown/Exit API and Handlers - -_client_ready = False - - -def is_initialized(): - """ - Get the initialization state of the local node. If True, node has - been configured. - @return: True if local node initialized - @rtype: bool - """ - return _client_ready -def set_initialized(initialized): - """ - set the initialization state of the local node - @param initialized: True if node initialized - @type initialized: bool - """ - global _client_ready - _client_ready = initialized - -_shutdown_lock = threading.RLock() - -# _shutdown_flag flags that rospy is in shutdown mode, in_shutdown -# flags that the shutdown routine has started. These are separate -# because 'pre-shutdown' hooks require rospy to be in a non-shutdown -# mode. These hooks are executed during the shutdown routine. -_shutdown_flag = False -_in_shutdown = False - -# various hooks to call on shutdown. shutdown hooks are called in the -# shutdown state, preshutdown are called just before entering shutdown -# state, and client shutdown is called before both of these. -_shutdown_hooks = [] -_preshutdown_hooks = [] -_client_shutdown_hooks = [] -# threads that must be joined on shutdown -_shutdown_threads = [] - -_signalChain = {} - -def is_shutdown(): - """ - @return: True if shutdown flag has been set - @rtype: bool - """ - return _shutdown_flag - -def _add_shutdown_hook(h, hooks): - """ - shared implementation of add_shutdown_hook and add_preshutdown_hook - """ - if type(h) not in [types.FunctionType, types.MethodType]: - raise TypeError("shutdown hook [%s] must be a function: %s"%(h, type(h))) - if _shutdown_flag: - _logger.warn("add_shutdown_hook called after shutdown") - h("already shutdown") - return - with _shutdown_lock: - if hooks is None: - # race condition check, don't log as we are deep into shutdown - return - hooks.append(h) - -def _add_shutdown_thread(t): - """ - Register thread that must be joined() on shutdown - """ - if _shutdown_flag: - #TODO - return - with _shutdown_lock: - if _shutdown_threads is None: - # race condition check, don't log as we are deep into shutdown - return - # in order to prevent memory leaks, reap dead threads. The - # last thread may not get reaped until shutdown, but this is - # relatively minor - for other in _shutdown_threads[:]: - if not other.isAlive(): - _shutdown_threads.remove(other) - _shutdown_threads.append(t) - -def add_client_shutdown_hook(h): - """ - Add client method to invoke when system shuts down. Unlike - L{add_shutdown_hook} and L{add_preshutdown_hooks}, these methods - will be called before any rospy internal shutdown code. - - @param h: function that takes in a single string argument (shutdown reason) - @type h: fn(str) - """ - _add_shutdown_hook(h, _client_shutdown_hooks) - -def add_preshutdown_hook(h): - """ - Add method to invoke when system shuts down. Unlike - L{add_shutdown_hook}, these methods will be called before any - other shutdown hooks. - - @param h: function that takes in a single string argument (shutdown reason) - @type h: fn(str) - """ - _add_shutdown_hook(h, _preshutdown_hooks) - -def add_shutdown_hook(h): - """ - Add method to invoke when system shuts down. - - Shutdown hooks are called in the order that they are - registered. This is an internal API method that is used to - cleanup. See the client X{on_shutdown()} method if you wish to - register client hooks. - - @param h: function that takes in a single string argument (shutdown reason) - @type h: fn(str) - """ - _add_shutdown_hook(h, _shutdown_hooks) - -def signal_shutdown(reason): - """ - Initiates shutdown process by signaling objects waiting on _shutdown_lock. - Shutdown and pre-shutdown hooks are invoked. - @param reason: human-readable shutdown reason, if applicable - @type reason: str - """ - global _shutdown_flag, _in_shutdown, _shutdown_lock, _shutdown_hooks - _logger.info("signal_shutdown [%s]"%reason) - if _shutdown_flag or _in_shutdown: - return - with _shutdown_lock: - if _shutdown_flag or _in_shutdown: - return - _in_shutdown = True - - # make copy just in case client re-invokes shutdown - for h in _client_shutdown_hooks: - try: - # client shutdown hooks do not accept a reason arg - h() - except: - traceback.print_exc() - del _client_shutdown_hooks[:] - - for h in _preshutdown_hooks: - try: - h(reason) - except: - traceback.print_exc() - del _preshutdown_hooks[:] - - # now that pre-shutdown hooks have been called, raise shutdown - # flag. This allows preshutdown hooks to still publish and use - # service calls properly - _shutdown_flag = True - for h in _shutdown_hooks: - try: - h(reason) - except Exception, e: - print >> sys.stderr, "signal_shutdown hook error[%s]"%e - del _shutdown_hooks[:] - - threads = _shutdown_threads[:] - - for t in threads: - if t.isAlive(): - t.join(_TIMEOUT_SHUTDOWN_JOIN) - del _shutdown_threads[:] - try: - time.sleep(0.1) #hack for now until we get rid of all the extra threads - except KeyboardInterrupt: pass - -def _ros_signal(sig, stackframe): - signal_shutdown("signal-"+str(sig)) - prev_handler = _signalChain.get(sig, None) - if prev_handler is not None and not type(prev_handler) == int: - try: - prev_handler(sig, stackframe) - except KeyboardInterrupt: - pass #filter out generic keyboard interrupt handler - -def _ros_atexit(): - signal_shutdown('atexit') -atexit.register(_ros_atexit) - -# #687 -def register_signals(): - """ - register system signal handlers for SIGTERM and SIGINT - """ - _signalChain[signal.SIGTERM] = signal.signal(signal.SIGTERM, _ros_signal) - _signalChain[signal.SIGINT] = signal.signal(signal.SIGINT, _ros_signal) - -# Validators ###################################### - -def is_topic(param_name): - """ - Validator that checks that parameter is a valid ROS topic name - """ - def validator(param_value, caller_id): - v = valid_name_validator_resolved(param_name, param_value, caller_id) - if param_value == '/': - raise ParameterInvalid("ERROR: parameter [%s] cannot be the global namespace"%param_name) - return v - return validator - -_proxies = {} #cache ServerProxys -def xmlrpcapi(uri): - """ - @return: instance for calling remote server or None if not a valid URI - @rtype: xmlrpclib.ServerProxy - """ - if uri is None: - return None - uriValidate = urlparse.urlparse(uri) - if not uriValidate[0] or not uriValidate[1]: - return None - if not _proxies.has_key(uri): - _proxies[uri] = xmlrpclib.ServerProxy(uri) - return _proxies[uri] - diff --git a/core/rospy/src/rospy/exceptions.py b/core/rospy/src/rospy/exceptions.py deleted file mode 100644 index f29ce7a8..00000000 --- a/core/rospy/src/rospy/exceptions.py +++ /dev/null @@ -1,88 +0,0 @@ -# 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$ - -"""rospy exception types""" - -class ROSException(Exception): - """ - Base exception class for ROS clients - """ - pass - -class ROSSerializationException(ROSException): - """ - Exception for message serialization errors - """ - pass - -class ROSInitException(ROSException): - """ - Exception for errors initializing ROS state - """ - pass - -class ROSInterruptException(ROSException, KeyboardInterrupt): - """ - Exception for operations that interrupted, e.g. due to shutdown. - - This is a subclass of both L{ROSException} and KeyboardInterrupt - so that it can be used in logic tht expects either. - """ - pass - -class ROSInternalException(Exception): - """ - Base class for exceptions that are internal to the ROS system - """ - pass - -class TransportException(ROSInternalException): - """ - Base class for transport-related exceptions - """ - pass - -class TransportTerminated(TransportException): - """ - Internal class for representing broken connections - """ - pass - -class TransportInitError(TransportException): - """ - Internal exception for representing exceptions that occur - establishing transports - """ - pass - diff --git a/core/rospy/src/rospy/impl/__init__.py b/core/rospy/src/rospy/impl/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/core/rospy/src/rospy/impl/init.py b/core/rospy/src/rospy/impl/init.py deleted file mode 100644 index 8af7fc03..00000000 --- a/core/rospy/src/rospy/impl/init.py +++ /dev/null @@ -1,147 +0,0 @@ -# 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$ -""" -Internal use: rospy initialization. - -This is mainly routines for initializing the master or slave based on -the OS environment. -""" - -import os -import sys -import logging -import time -import traceback - -import roslib.rosenv -import roslib.xmlrpc - -from ..names import _set_caller_id -from ..core import is_shutdown, add_log_handler, signal_shutdown, rospyerr -from ..rostime import is_wallclock, get_time - -from .tcpros import init_tcpros -from .masterslave import ROSHandler - -DEFAULT_NODE_PORT = 0 #bind to any open port -DEFAULT_MASTER_PORT=11311 #default port for master's to bind to -DEFAULT_MASTER_URI = 'http://localhost:%s/'%DEFAULT_MASTER_PORT - -################################################### -# rospy module lower-level initialization - -def _node_run_error(e): - """ - If XML-RPC errors out of the run() method, this handler is invoked - """ - rospyerr(traceback.format_exc()) - signal_shutdown('error in XML-RPC server: %s'%(e)) - -def start_node(environ, resolved_name, master_uri=None, port=None): - """ - Load ROS slave node, initialize from environment variables - @param environ: environment variables - @type environ: dict - @param resolved_name: resolved node name - @type resolved_name: str - @param master_uri: override ROS_MASTER_URI: XMlRPC URI of central ROS server - @type master_uri: str - @param port: override ROS_PORT: port of slave xml-rpc node - @type port: int - @return: node server instance - @rtype roslib.xmlrpc.XmlRpcNode - @raise ROSInitException: if node has already been started - """ - init_tcpros() - if not master_uri: - master_uri = roslib.rosenv.get_master_uri() - if not master_uri: - master_uri = DEFAULT_MASTER_URI - - # this will go away in future versions of API - _set_caller_id(resolved_name) - - handler = ROSHandler(resolved_name, master_uri) - node = roslib.xmlrpc.XmlRpcNode(port, handler, on_run_error=_node_run_error) - node.start() - while not node.uri and not is_shutdown(): - time.sleep(0.00001) #poll for XMLRPC init - logging.getLogger("rospy.init").info("ROS Slave URI: [%s]", node.uri) - - while not handler._is_registered() and not is_shutdown(): - time.sleep(0.1) #poll for master registration - logging.getLogger("rospy.init").info("registered with master") - return node - -# #2879 -# resolve sys.stdout/stderr each time through in case testing program or otherwise wants to redirect stream -def _stdout_log(level): - def fn(msg): - if is_wallclock(): - sys.stdout.write("[%s] [WallTime: %f] %s\n"%(level,time.time(), str(msg))) - else: - sys.stdout.write("[%s] [WallTime: %f] [%f] %s\n"%(level,time.time(), get_time(), str(msg))) - return fn -def _stderr_log(level): - def fn(msg): - if is_wallclock(): - sys.stderr.write("[%s] [WallTime: %f] %s\n"%(level,time.time(), str(msg))) - else: - sys.stderr.write("[%s] [WallTime: %f] [%f] %s\n"%(level,time.time(), get_time(), str(msg))) - return fn - -_loggers_initialized = False -def init_log_handlers(): - global _loggers_initialized - if _loggers_initialized: - return - - from roslib.msg import Log - - # client logger - clogger = logging.getLogger("rosout") - add_log_handler(Log.DEBUG, clogger.debug) - add_log_handler(Log.INFO, clogger.info) - add_log_handler(Log.ERROR, clogger.error) - add_log_handler(Log.WARN, clogger.warn) - add_log_handler(Log.FATAL, clogger.critical) - - # TODO: make this configurable - # INFO -> stdout - # ERROR, FATAL -> stderr - add_log_handler(Log.INFO, _stdout_log('INFO')) - add_log_handler(Log.WARN, _stderr_log('WARN')) - add_log_handler(Log.ERROR, _stderr_log('ERROR')) - add_log_handler(Log.FATAL, _stderr_log('FATAL')) - diff --git a/core/rospy/src/rospy/impl/masterslave.py b/core/rospy/src/rospy/impl/masterslave.py deleted file mode 100644 index 8d9097f9..00000000 --- a/core/rospy/src/rospy/impl/masterslave.py +++ /dev/null @@ -1,523 +0,0 @@ -# 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$ -""" -Internal use: ROS Node (Slave) API. - -The Node API is implemented by the L{ROSHandler}. - -API return convention: (statusCode, statusMessage, returnValue) - - - statusCode: an integer indicating the completion condition of the method. - - statusMessage: a human-readable string message for debugging - - returnValue: the return value of the method; method-specific. - -Current status codes: - - - -1: ERROR: Error on the part of the caller, e.g. an invalid parameter - - 0: FAILURE: Method was attempted but failed to complete correctly. - - 1: SUCCESS: Method completed successfully. - -Individual methods may assign additional meaning/semantics to statusCode. -""" - -import os -import sys -import itertools -import logging -import socket -import threading -import traceback -import time -import urlparse - -from roslib.xmlrpc import XmlRpcHandler - -import rospy.names -import rospy.rostime - -import rospy.impl.tcpros - -from rospy.core import * -from rospy.impl.paramserver import get_param_server_cache -from rospy.impl.registration import RegManager, get_topic_manager -from rospy.impl.validators import non_empty, ParameterInvalid - -# Return code slots -STATUS = 0 -MSG = 1 -VAL = 2 - -# pseudo-validators ############################### -# these validators actually return tuples instead of a function and it is up to a custom -# validator on the class itself to perform the validation -def is_publishers_list(paramName): - return ('is_publishers_list', paramName) - -_logger = logging.getLogger("rospy.impl.masterslave") - -LOG_API = True - -def apivalidate(error_return_value, validators=()): - """ - ROS master/slave arg-checking decorator. Applies the specified - validator to the corresponding argument and also remaps each - argument to be the value returned by the validator. Thus, - arguments can be simultaneously validated and canonicalized prior - to actual function call. - @param error_return_value: API value to return if call unexpectedly fails - @param validators: sequence of validators to apply to each - arg. None means no validation for the parameter is required. As all - api methods take caller_id as the first parameter, the validators - start with the second param. - @type validators: sequence - """ - def check_validates(f): - assert len(validators) == f.func_code.co_argcount - 2, "%s failed arg check"%f #ignore self and caller_id - def validated_f(*args, **kwds): - if LOG_API: - _logger.debug("%s%s", f.func_name, str(args[1:])) - #print "%s%s"%(f.func_name, str(args[1:])) - if len(args) == 1: - _logger.error("%s invoked without caller_id paramter"%f.func_name) - return -1, "missing required caller_id parameter", error_return_value - elif len(args) != f.func_code.co_argcount: - return -1, "Error: bad call arity", error_return_value - - instance = args[0] - caller_id = args[1] - if not isinstance(caller_id, basestring): - _logger.error("%s: invalid caller_id param type", f.func_name) - return -1, "caller_id must be a string", error_return_value - - newArgs = [instance, caller_id] #canonicalized args - try: - for (v, a) in itertools.izip(validators, args[2:]): - if v: - try: - #simultaneously validate + canonicalized args - if type(v) == list or type(v) == tuple: - newArgs.append(instance._custom_validate(v[0], v[1], a, caller_id)) - else: - newArgs.append(v(a, caller_id)) - except ParameterInvalid, e: - _logger.error("%s: invalid parameter: %s", f.func_name, str(e) or 'error') - return -1, str(e) or 'error', error_return_value - else: - newArgs.append(a) - - if LOG_API: - retval = f(*newArgs, **kwds) - _logger.debug("%s%s returns %s", f.func_name, args[1:], retval) - return retval - else: - code, msg, val = f(*newArgs, **kwds) - if val is None: - return -1, "Internal error (None value returned)", error_return_value - return code, msg, val - except TypeError, te: #most likely wrong arg number - _logger.error(traceback.format_exc()) - return -1, "Error: invalid arguments: %s"%te, error_return_value - except Exception, e: #internal failure - _logger.error(traceback.format_exc()) - return 0, "Internal failure: %s"%e, error_return_value - validated_f.func_name = f.func_name - validated_f.__doc__ = f.__doc__ #preserve doc - return validated_f - return check_validates - - -class ROSHandler(XmlRpcHandler): - """ - Base handler for both slave and master nodes. API methods - generally provide the capability for establishing point-to-point - connections with other nodes. - - Instance methods are XML-RPC API methods, so care must be taken as - to what is added here. - """ - - def __init__(self, name, master_uri): - """ - Base constructor for ROS nodes/masters - @param name: ROS name of this node - @type name: str - @param master_uri: URI of master node, or None if this node is the master - @type master_uri: str - """ - super(ROSHandler, self).__init__() - self.masterUri = master_uri - self.name = name - self.uri = None - self.done = False - - # initialize protocol handlers. The master will not have any. - self.protocol_handlers = [] - handler = rospy.impl.tcpros.get_tcpros_handler() - if handler is not None: - self.protocol_handlers.append(handler) - - self.reg_man = RegManager(self) - - ############################################################################### - # INTERNAL - - def _is_registered(self): - """ - @return: True if slave API is registered with master. - @rtype: bool - """ - if self.reg_man is None: - return False - else: - return self.reg_man.is_registered() - - - def _ready(self, uri): - """ - @param uri: XML-RPC URI - @type uri: str - callback from ROSNode to inform handler of correct i/o information - """ - _logger.info("_ready: %s", uri) - self.uri = uri - #connect up topics in separate thread - if self.reg_man: - t = threading.Thread(target=self.reg_man.start, args=(uri, self.masterUri)) - rospy.core._add_shutdown_thread(t) - t.start() - - def _custom_validate(self, validation, param_name, param_value, caller_id): - """ - Implements validation rules that require access to internal ROSHandler state. - @param validation: name of validation rule to use - @type validation: str - @param param_name: name of parameter being validated - @type param_name: str - @param param_value str: value of parameter - @type param_value: str - @param caller_id: value of caller_id parameter to API method - @type caller_id: str - @raise ParameterInvalid: if the parameter does not meet validation - @return: new value for parameter, after validation - """ - if validation == 'is_publishers_list': - if not type(param_value) == list: - raise ParameterInvalid("ERROR: param [%s] must be a list"%param_name) - for v in param_value: - if not isinstance(v, basestring): - raise ParameterInvalid("ERROR: param [%s] must be a list of strings"%param_name) - parsed = urlparse.urlparse(v) - if not parsed[0] or not parsed[1]: #protocol and host - raise ParameterInvalid("ERROR: param [%s] does not contain valid URLs [%s]"%(param_name, v)) - return param_value - else: - raise ParameterInvalid("ERROR: param [%s] has an unknown validation type [%s]"%(param_name, validation)) - - ## static map for tracking which arguments to a function should be remapped - # { methodName : [ arg indices ] - _remap_table = { } - - @classmethod - def remappings(cls, methodName): - """ - @internal - @param cls: class to register remappings on - @type cls: Class: class to register remappings on - @return: parameters (by pos) that should be remapped because they are names - @rtype: list - """ - if methodName in cls._remap_table: - return cls._remap_table[methodName] - else: - return [] - - ############################################################################### - # UNOFFICIAL/PYTHON-ONLY API - - @apivalidate('') - ## (Python-Only API) Get the XML-RPC URI of this server - ## @param self - ## @param caller_id str: ROS caller id - ## @return [int, str, str]: [1, "", xmlRpcUri] - def getUri(self, caller_id): - return 1, "", self.uri - - @apivalidate('') - ## (Python-Only API) Get the ROS node name of this server - ## @param self - ## @param caller_id str: ROS caller id - ## @return [int, str, str]: [1, "", ROS node name] - def getName(self, caller_id): - return 1, "", self.name - - - ############################################################################### - # EXTERNAL API - - @apivalidate([]) - def getBusStats(self, caller_id): - """ - Retrieve transport/topic statistics - @param caller_id: ROS caller id - @type caller_id: str - @return: [publishStats, subscribeStats, serviceStats]:: - publishStats: [[topicName, messageDataSent, pubConnectionData]...[topicNameN, messageDataSentN, pubConnectionDataN]] - pubConnectionData: [connectionId, bytesSent, numSent, connected]* . - subscribeStats: [[topicName, subConnectionData]...[topicNameN, subConnectionDataN]] - subConnectionData: [connectionId, bytesReceived, dropEstimate, connected]* . dropEstimate is -1 if no estimate. - serviceStats: not sure yet, probably akin to [numRequests, bytesReceived, bytesSent] - """ - pub_stats, sub_stats = get_topic_manager().get_pub_sub_stats() - #TODO: serviceStats - return 1, '', [pub_stats, sub_stats, []] - - @apivalidate([]) - def getBusInfo(self, caller_id): - """ - Retrieve transport/topic connection information - @param caller_id: ROS caller id - @type caller_id: str - """ - return 1, "bus info", get_topic_manager().get_pub_sub_info() - - @apivalidate('') - def getMasterUri(self, caller_id): - """ - Get the URI of the master node. - @param caller_id: ROS caller id - @type caller_id: str - @return: [code, msg, masterUri] - @rtype: [int, str, str] - """ - if self.masterUri: - return 1, self.masterUri, self.masterUri - else: - return 0, "master URI not set", "" - - def _shutdown(self, reason=''): - """ - @param reason: human-readable debug string - @type reason: str - """ - if not self.done: - self.done = True - if reason: - _logger.info(reason) - if self.protocol_handlers: - for handler in self.protocol_handlers: - handler.shutdown() - del self.protocol_handlers[:] - self.protocol_handlers = None - return True - - @apivalidate(0, (None, )) - def shutdown(self, caller_id, msg=''): - """ - Stop this server - @param caller_id: ROS caller id - @type caller_id: str - @param msg: a message describing why the node is being shutdown. - @type msg: str - @return: [code, msg, 0] - @rtype: [int, str, int] - """ - if msg: - print >> sys.stdout, "shutdown request: %s"%msg - else: - print >> sys.stdout, "shutdown requst" - if self._shutdown('external shutdown request from [%s]: %s'%(caller_id, msg)): - signal_shutdown('external shutdown request from [%s]: [%s]'%(caller_id, msg)) - return 1, "shutdown", 0 - - @apivalidate(-1) - def getPid(self, caller_id): - """ - Get the PID of this server - @param caller_id: ROS caller id - @type caller_id: str - @return: [1, "", serverProcessPID] - @rtype: [int, str, int] - """ - return 1, "", os.getpid() - - ############################################################################### - # PUB/SUB APIS - - @apivalidate([]) - def getSubscriptions(self, caller_id): - """Retrieve a list of topics that this node subscribes to - @param caller_id: ROS caller id - @type caller_id: str - @return [int, str, [ [topic1, topicType1]...[topicN, topicTypeN]]]: list of topics this node subscribes to - """ - return 1, "subscriptions", get_topic_manager().get_subscriptions() - - @apivalidate([]) - def getPublications(self, caller_id): - """ - Retrieve a list of topics that this node publishes. - @param caller_id: ROS caller id - @type caller_id: str - @return: list of topics published by this node - @rtype: [int, str, [ [topic1, topicType1]...[topicN, topicTypeN]]] - """ - return 1, "publications", get_topic_manager().get_publications() - - def _connect_topic(self, topic, pub_uri): - """ - Connect subscriber to topic - @param topic: topic name to connect - @type topic: str - @param pub_uri: API URI of topic publisher - @type pub_uri: str - @return: [code, msg, numConnects]. numConnects is the number - of subscribers connected to the topic - @rtype: [int, str, int] - """ - caller_id = rospy.names.get_caller_id() - sub = get_topic_manager().get_subscriber_impl(topic) - if not sub: - return -1, "No subscriber for topic [%s]"%topic, 0 - elif sub.has_connection(pub_uri): - return 1, "_connect_topic[%s]: subscriber already connected to publisher [%s]"%(topic, pub_uri), 0 - - #Negotiate with source for connection - # - collect supported protocols - protocols = [] - for h in self.protocol_handlers: #currently only TCPROS - protocols.extend(h.get_supported()) - if not protocols: - return 0, "ERROR: no available protocol handlers", 0 - - _logger.debug("connect[%s]: calling requestTopic(%s, %s, %s)", topic, caller_id, topic, str(protocols)) - # 1) have to preserve original (unresolved) params as this may - # go outside our graph - # 2) xmlrpclib doesn't give us any way of affecting the - # timeout other than affecting the global timeout. We need - # to set a timeout to prevent infinite hangs. 60 seconds is - # a *very* long time. All of the rospy code right now sets - # individual socket timeouts, but this could potentially - # affect user code. - socket.setdefaulttimeout(60.) - try: - code, msg, result = \ - xmlrpcapi(pub_uri).requestTopic(caller_id, topic, protocols) - except Exception, e: - return 0, "unable to requestTopic: %s"%str(e), 0 - - #Create the connection (if possible) - if code <= 0: - _logger.debug("connect[%s]: requestTopic did not succeed %s, %s", pub_uri, code, msg) - return code, msg, 0 - elif not result or type(protocols) != list: - return 0, "ERROR: publisher returned invalid protocol choice: %s"%(str(result)), 0 - _logger.debug("connect[%s]: requestTopic returned protocol list %s", topic, result) - protocol = result[0] - for h in self.protocol_handlers: - if h.supports(protocol): - return h.create_transport(topic, pub_uri, result) - return 0, "ERROR: publisher returned unsupported protocol choice: %s"%result, 0 - - @apivalidate(-1, (global_name('parameter_key'), None)) - def paramUpdate(self, caller_id, parameter_key, parameter_value): - """ - Callback from master of current publisher list for specified topic. - @param caller_id: ROS caller id - @type caller_id: str - @param parameter_key str: parameter name, globally resolved - @type parameter_key: str - @param parameter_value New parameter value - @type parameter_value: XMLRPC-legal value - @return: [code, status, ignore]. If code is -1 ERROR, the node - is not subscribed to parameter_key - @rtype: [int, str, int] - """ - try: - get_param_server_cache().update(parameter_key, parameter_value) - return 1, '', 0 - except KeyError: - return -1, 'not subscribed', 0 - - @apivalidate(-1, (is_topic('topic'), is_publishers_list('publishers'))) - def publisherUpdate(self, caller_id, topic, publishers): - """ - Callback from master of current publisher list for specified topic. - @param caller_id: ROS caller id - @type caller_id: str - @param topic str: topic name - @type topic: str - @param publishers: list of current publishers for topic in the form of XMLRPC URIs - @type publishers: [str] - @return: [code, status, ignore] - @rtype: [int, str, int] - """ - if self.reg_man: - for uri in publishers: - self.reg_man.publisher_update(topic, publishers) - return 1, "", 0 - - _remap_table['requestTopic'] = [0] # remap topic - @apivalidate([], (is_topic('topic'), non_empty('protocols'))) - def requestTopic(self, caller_id, topic, protocols): - """ - Publisher node API method called by a subscriber node. - - Request that source allocate a channel for communication. Subscriber provides - a list of desired protocols for communication. Publisher returns the - selected protocol along with any additional params required for - establishing connection. For example, for a TCP/IP-based connection, - the source node may return a port number of TCP/IP server. - @param caller_id str: ROS caller id - @type caller_id: str - @param topic: topic name - @type topic: str - @param protocols: list of desired - protocols for communication in order of preference. Each - protocol is a list of the form [ProtocolName, - ProtocolParam1, ProtocolParam2...N] - @type protocols: [[str, XmlRpcLegalValue*]] - @return: [code, msg, protocolParams]. protocolParams may be an - empty list if there are no compatible protocols. - @rtype: [int, str, [str, XmlRpcLegalValue*]] - """ - if not get_topic_manager().has_publication(topic): - return -1, "Not a publisher of [%s]"%topic, [] - for protocol in protocols: #simple for now: select first implementation - protocol_id = protocol[0] - for h in self.protocol_handlers: - if h.supports(protocol_id): - _logger.debug("requestTopic[%s]: choosing protocol %s", topic, protocol_id) - return h.init_publisher(topic, protocol) - return 0, "no supported protocol implementations", [] - diff --git a/core/rospy/src/rospy/impl/paramserver.py b/core/rospy/src/rospy/impl/paramserver.py deleted file mode 100644 index 21043d72..00000000 --- a/core/rospy/src/rospy/impl/paramserver.py +++ /dev/null @@ -1,108 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, 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. - -"""Parameter Server Cache""" - -import threading - -class ParamServerCache(object): - """ - Cache of values on the parameter server. Implementation - is just a thread-safe dictionary. - """ - - def __init__(self): - self.lock = threading.Lock() - self.d = {} - - ## Delete parameter from cache - def delete(self, key): - try: - self.lock.acquire() - del self.d[key] - finally: - self.lock.release() - - def update(self, key, value): - """ - Update the value of the parameter in the cache - @param key: parameter key - @type key: str - @param value: parameter value - @type value: str - @raise: KeyError if key is not already in the cache. - """ - if not key in self.d: - raise KeyError(key) - try: - self.lock.acquire() - self.d[key] = value - finally: - self.lock.release() - - def set(self, key, value): - """ - Set the value of the parameter in the cache. This is a - prerequisite of calling update(). - @param key: parameter key - @type key: str - @param value: parameter value - @type value: str - """ - try: - self.lock.acquire() - self.d[key] = value - finally: - self.lock.release() - - def get(self, key): - """ - @param key: parameter key - @type key: str - @return: Current value for parameter - @raise: KeyError - """ - try: - self.lock.acquire() - return self.d[key] - finally: - self.lock.release() - -_param_server_cache = None -def get_param_server_cache(): - """ - Get a handle on the client-wide parameter server cache - """ - global _param_server_cache - if _param_server_cache is None: - _param_server_cache = ParamServerCache() - return _param_server_cache diff --git a/core/rospy/src/rospy/impl/registration.py b/core/rospy/src/rospy/impl/registration.py deleted file mode 100644 index 868f433d..00000000 --- a/core/rospy/src/rospy/impl/registration.py +++ /dev/null @@ -1,471 +0,0 @@ -# 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$ - -"""Internal use: handles maintaining registrations with master via internal listener APIs""" - -from __future__ import with_statement - -import socket -import sys -import logging -import thread -import threading -import time -import traceback -import xmlrpclib - -from rospy.core import is_shutdown, xmlrpcapi, \ - logfatal, logwarn, loginfo, logerr, logdebug, \ - signal_shutdown, add_preshutdown_hook -from rospy.names import get_caller_id, get_namespace - -# topic manager and service manager singletons - -_topic_manager = None -def set_topic_manager(tm): - global _topic_manager - _topic_manager = tm -def get_topic_manager(): - return _topic_manager - -_service_manager = None -def set_service_manager(sm): - global _service_manager - _service_manager = sm -def get_service_manager(): - return _service_manager - - -class Registration(object): - """Registration types""" - PUB = 'pub' - SUB = 'sub' - SRV = 'srv' - -class RegistrationListener(object): - """Listener API for subscribing to changes in Publisher/Subscriber/Service declarations""" - - def reg_added(self, resolved_name, data_type_or_uri, reg_type): - """ - New pub/sub/service declared. - @param resolved_name: resolved topic/service name - @param data_type_or_uri: topic type or service uri - @type data_type_or_uri: str - @param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV} - @type reg_type: str - """ - pass - - def reg_removed(self, resolved_name, data_type_or_uri, reg_type): - """ - New pub/sub/service removed. - @param resolved_name: topic/service name - @type resolved_name: str - @param data_type_or_uri: topic type or service uri - @type data_type_or_uri: str - @param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV} - @type reg_type: str - """ - pass - -class RegistrationListeners(object): - - def __init__(self): - """ - ctor. - """ - self.listeners = [] - self.lock = threading.Lock() - - def add_listener(self, l): - """ - Subscribe to notifications of pub/sub/service registration - changes. This is an internal API used to notify higher level - routines when to communicate with the master. - @param l: listener to subscribe - @type l: TopicListener - """ - assert isinstance(l, RegistrationListener) - with self.lock: - self.listeners.append(l) - - def notify_removed(self, resolved_name, data_type_or_uri, reg_type): - """ - @param resolved_name: resolved_topic/service name - @type resolved_name: str - @param data_type_or_uri: topic type or service uri - @type data_type_or_uri: str - @param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV} - @type reg_type: str - """ - with self.lock: - for l in self.listeners: - try: - l.reg_removed(resolved_name, data_type_or_uri, reg_type) - except Exception, e: - logerr("error notifying listener of removal: %s"%traceback.format_exc(e)) - - def notify_added(self, resolved_name, data_type, reg_type): - """ - @param resolved_name: topic/service name - @type resolved_name: str - @param data_type: topic/service type - @type data_type: str - @param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV} - @type reg_type: str - """ - with self.lock: - for l in self.listeners: - try: - l.reg_added(resolved_name, data_type, reg_type) - except Exception, e: - logerr(traceback.format_exc(e)) - - def clear(self): - """ - Remove all registration listeners - """ - with self.lock: - del self.listeners[:] - -_registration_listeners = RegistrationListeners() -def get_registration_listeners(): - return _registration_listeners - -# RegManager's main purpose is to collect all client->master communication in one place - -class RegManager(RegistrationListener): - """ - Registration manager used by Node implemenation. - Communicates with ROS Master to maintain topic registration - information. Also responds to publisher updates to create topic - connections - """ - - def __init__(self, handler): - """ - ctor. - @param handler: node API handler - """ - self.logger = logging.getLogger("rospy.registration") - self.handler = handler - self.uri = self.master_uri = None - self.updates = [] - self.cond = threading.Condition() #for locking/notifying updates - self.registered = False - # cleanup has to occur before official shutdown - add_preshutdown_hook(self.cleanup) - - def start(self, uri, master_uri): - """ - Start the RegManager. This should be passed in as an argument to a thread - starter as the RegManager is designed to spin in its own thread - @param uri: URI of local node - @type uri: str - @param master_uri: Master URI - @type master_uri: str - """ - self.registered = False - self.master_uri = master_uri - self.uri = uri - first = True - tm = get_topic_manager() - sm = get_service_manager() - ns = get_namespace() - caller_id = get_caller_id() - if not master_uri or master_uri == uri: - registered = True - master = None - else: - registered = False - master = xmlrpcapi(master_uri) - self.logger.info("Registering with master node %s", master_uri) - - while not registered and not is_shutdown(): - try: - try: - # prevent TopicManager and ServiceManager from accepting registrations until we are done - tm.lock.acquire() - sm.lock.acquire() - - pub, sub, srv = tm.get_publications(), tm.get_subscriptions(), sm.get_services() - for resolved_name, data_type in pub: - self.logger.info("Registering publisher topic [%s] type [%s] with master", resolved_name, data_type) - code, msg, val = master.registerPublisher(caller_id, resolved_name, data_type, uri) - if code != 1: - logfatal("cannot register publication topic [%s] with master: %s"%(resolved_name, msg)) - signal_shutdown("master/node incompatibility with register publisher") - for resolved_name, data_type in sub: - self.logger.info("registering subscriber topic [%s] type [%s] with master", resolved_name, data_type) - code, msg, val = master.registerSubscriber(caller_id, resolved_name, data_type, uri) - if code != 1: - logfatal("cannot register subscription topic [%s] with master: %s"%(resolved_name, msg)) - signal_shutdown("master/node incompatibility with register subscriber") - else: - self.publisher_update(resolved_name, val) - for resolved_name, service_uri in srv: - self.logger.info("registering service [%s] uri [%s] with master", resolved_name, service_uri) - code, msg, val = master.registerService(caller_id, resolved_name, service_uri, uri) - if code != 1: - logfatal("cannot register service [%s] with master: %s"%(resolved_name, msg)) - signal_shutdown("master/node incompatibility with register service") - - registered = True - - # Subscribe to updates to our state - get_registration_listeners().add_listener(self) - finally: - sm.lock.release() - tm.lock.release() - - if pub or sub: - logdebug("Registered [%s] with master node %s", caller_id, master_uri) - else: - logdebug("No topics to register with master node %s", master_uri) - - except Exception, e: - if first: - # this use to print to console always, arguable whether or not this should be subjected to same configuration options as logging - logerr("Unable to immediately register with master node [%s]: master may not be running yet. Will keep trying."%master_uri) - first = False - time.sleep(0.2) - self.registered = True - self.run() - - def is_registered(self): - """ - Check if Node has been registered yet. - @return: True if registration has occurred with master - @rtype: bool - """ - return self.registered - - def run(self): - """ - Main RegManager thread loop. - Periodically checks the update - queue and generates topic connections - """ - #Connect the topics - while not self.handler.done and not is_shutdown(): - cond = self.cond - try: - cond.acquire() - if not self.updates: - cond.wait(0.5) - if self.updates: - #work from the end as these are the most up-to-date - topic, uris = self.updates.pop() - #filter out older updates for same topic - self.updates = [x for x in self.updates if x[0] != topic] - else: - topic = uris = None - finally: - if cond is not None: - cond.release() - - #call _connect_topic on all URIs as it can check to see whether - #or not a connection exists. - if uris and not self.handler.done: - for uri in uris: - # #1141: have to multithread this to prevent a bad publisher from hanging us - thread.start_new_thread(self._connect_topic_thread, (topic, uri)) - - def _connect_topic_thread(self, topic, uri): - try: - code, msg, _ = self.handler._connect_topic(topic, uri) - if code != 1: - logdebug("Unable to connect subscriber to publisher [%s] for topic [%s]: %s", uri, topic, msg) - except Exception, e: - if not is_shutdown(): - logdebug("Unable to connect to publisher [%s] for topic [%s]: %s"%(uri, topic, traceback.format_exc())) - - def cleanup(self, reason): - """ - Cleans up registrations with master and releases topic and service resources - @param reason: human-reasonable debug string - @type reason: str - """ - self.logger.debug("registration cleanup starting") - try: - self.cond.acquire() - self.cond.notifyAll() - finally: - self.cond.release() - - # we never successfully initialized master_uri - if not self.master_uri: - return - - master = xmlrpcapi(self.master_uri) - # we never successfully initialized master - if master is None: - return - - caller_id = get_caller_id() - - # clear the registration listeners as we are going to do a quick unregister here - rl = get_registration_listeners() - if rl is not None: - rl.clear() - - tm = get_topic_manager() - sm = get_service_manager() - try: - multi = xmlrpclib.MultiCall(master) - if tm is not None: - for resolved_name, _ in tm.get_subscriptions(): - self.logger.debug("unregisterSubscriber [%s]"%resolved_name) - multi.unregisterSubscriber(caller_id, resolved_name, self.uri) - for resolved_name, _ in tm.get_publications(): - self.logger.debug("unregisterPublisher [%s]"%resolved_name) - multi.unregisterPublisher(caller_id, resolved_name, self.uri) - - if sm is not None: - for resolved_name, service_uri in sm.get_services(): - self.logger.debug("unregisterService [%s]"%resolved_name) - multi.unregisterService(caller_id, resolved_name, service_uri) - multi() - except socket.error, (errno, msg): - if errno == 111 or errno == 61: #can't talk to master, nothing we can do about it - self.logger.warn("cannot unregister with master due to network issues") - else: - self.logger.warn("unclean shutdown\n%s"%traceback.format_exc()) - except: - self.logger.warn("unclean shutdown\n%s"%traceback.format_exc()) - - self.logger.debug("registration cleanup: master calls complete") - - #TODO: cleanup() should actually be orchestrated by a separate - #cleanup routine that calls the reg manager/sm/tm - if tm is not None: - tm.close_all() - if sm is not None: - sm.unregister_all() - - def reg_removed(self, resolved_name, data_type_or_uri, reg_type): - """ - RegistrationListener callback - @param resolved_name: resolved name of topic or service - @type resolved_name: str - @param data_type_or_uri: either the data type (for topic regs) or the service URI (for service regs). - @type data_type_or_uri: str - @param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV} - @type reg_type: str - """ - master_uri = self.master_uri - if not master_uri: - self.logger.error("Registrar: master_uri is not set yet, cannot inform master of deregistration") - else: - try: - master = xmlrpcapi(master_uri) - if reg_type == Registration.PUB: - self.logger.debug("unregisterPublisher(%s, %s)", resolved_name, self.uri) - master.unregisterPublisher(get_caller_id(), resolved_name, self.uri) - elif reg_type == Registration.SUB: - self.logger.debug("unregisterSubscriber(%s, %s)", resolved_name, data_type_or_uri) - master.unregisterSubscriber(get_caller_id(), resolved_name, self.uri) - elif reg_type == Registration.SRV: - self.logger.debug("unregisterService(%s, %s)", resolved_name, data_type_or_uri) - master.unregisterService(get_caller_id(), resolved_name, data_type_or_uri) - except: - logwarn("unable to communicate with ROS Master, registrations are now out of sync") - self.logger.error(traceback.format_exc()) - - def reg_added(self, resolved_name, data_type_or_uri, reg_type): - """ - RegistrationListener callback - @param resolved_name: resolved name of topic or service - @type resolved_name: str - @param data_type_or_uri: either the data type (for topic regs) or the service URI (for service regs). - @type data_type_or_uri: str - @param reg_type: Valid values are L{Registration.PUB}, L{Registration.SUB}, L{Registration.SRV} - @type reg_type: str - """ - #TODO: this needs to be made robust to master outages - master_uri = self.master_uri - if not master_uri: - self.logger.error("Registrar: master_uri is not set yet, cannot inform master of registration") - else: - master = xmlrpcapi(master_uri) - args = (get_caller_id(), resolved_name, data_type_or_uri, self.uri) - registered = False - first = True - while not registered and not is_shutdown(): - try: - if reg_type == Registration.PUB: - self.logger.debug("master.registerPublisher(%s, %s, %s, %s)"%args) - code, msg, val = master.registerPublisher(*args) - if code != 1: - logfatal("unable to register publication [%s] with master: %s"%(resolved_name, msg)) - elif reg_type == Registration.SUB: - self.logger.debug("master.registerSubscriber(%s, %s, %s, %s)"%args) - code, msg, val = master.registerSubscriber(*args) - if code == 1: - self.publisher_update(resolved_name, val) - else: - # this is potentially worth exiting over. in the future may want to add a retry - # timer - logfatal("unable to register subscription [%s] with master: %s"%(resolved_name, msg)) - elif reg_type == Registration.SRV: - self.logger.debug("master.registerService(%s, %s, %s, %s)"%args) - code, msg, val = master.registerService(*args) - if code != 1: - logfatal("unable to register service [%s] with master: %s"%(resolved_name, msg)) - - registered = True - except Exception, e: - if first: - msg = "Unable to register with master node [%s]: master may not be running yet. Will keep trying."%master_uri - self.logger.error(str(e)+"\n"+msg) - print msg - first = False - time.sleep(0.2) - - def publisher_update(self, resolved_name, uris): - """ - Inform psmanager of latest publisher list for a topic. This - will cause L{RegManager} to create a topic connection for all new - publishers (in a separate thread). - @param resolved_name: resolved topic name - @type resolved_name: str - @param uris: list of all publishers uris for topic - @type uris: [str] - """ - try: - self.cond.acquire() - self.updates.append((resolved_name, uris)) - self.cond.notifyAll() - finally: - self.cond.release() diff --git a/core/rospy/src/rospy/impl/rosout.py b/core/rospy/src/rospy/impl/rosout.py deleted file mode 100644 index 58e0fe08..00000000 --- a/core/rospy/src/rospy/impl/rosout.py +++ /dev/null @@ -1,116 +0,0 @@ -# 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$ - -"""Internal use: support for /rosout logging in rospy""" - -import logging -import sys -import traceback - -import rospy.names - -from rospy.core import add_log_handler, get_caller_id -from rospy.exceptions import ROSException -from rospy.topics import Publisher, Subscriber -from rospy.rostime import Time - -from rospy.impl.registration import get_topic_manager - -#Log message for rosout -import roslib.msg - -_ROSOUT = '/rosout' -_rosout_pub = None - -def init_rosout(): - logger = logging.getLogger("rospy.rosout") - try: - global _rosout_pub - if _rosout_pub is None: - logger.info("initializing %s core topic"%_ROSOUT) - _rosout_pub = Publisher(_ROSOUT, roslib.msg.Log, latch=True) - logger.info("connected to core topic %s"%_ROSOUT) - return True - except Exception, e: - logger.error("Unable to initialize %s: %s\n%s", _ROSOUT, e, traceback.format_exc()) - return False - -_in_rosout = False -## log an error to the /rosout topic -def _rosout(level, msg): - global _in_rosout - try: - if _rosout_pub is not None: - # protect against infinite recursion - if not _in_rosout: - try: - _in_rosout = True - msg = str(msg) - topics = get_topic_manager().get_topics() - l = roslib.msg.Log(level=level, name=str(rospy.names.get_caller_id()), msg=str(msg), topics=topics) - l.header.stamp = Time.now() - _rosout_pub.publish(l) - finally: - _in_rosout = False - except Exception, e: - #traceback.print_exc() - # don't use logerr in this case as that is recursive here - logger = logging.getLogger("rospy.rosout") - logger.error("Unable to report rosout: %s\n%s", e, traceback.format_exc()) - return False - -def _rosout_debug(msg): - _rosout(roslib.msg.Log.DEBUG, msg) -def _rosout_info(msg): - _rosout(roslib.msg.Log.INFO, msg) -def _rosout_warn(msg): - _rosout(roslib.msg.Log.WARN, msg) -def _rosout_error(msg): - _rosout(roslib.msg.Log.ERROR, msg) -def _rosout_fatal(msg): - _rosout(roslib.msg.Log.FATAL, msg) - -## Load loggers for publishing to /rosout -## @param level int: roslib.msg.Log level. Loggers >= level will be loaded. -def load_rosout_handlers(level): - if roslib.msg.Log.DEBUG >= level: - add_log_handler(roslib.msg.Log.DEBUG, _rosout_debug) - if roslib.msg.Log.INFO >= level: - add_log_handler(roslib.msg.Log.INFO, _rosout_info) - if roslib.msg.Log.WARN >= level: - add_log_handler(roslib.msg.Log.WARN, _rosout_warn) - if roslib.msg.Log.ERROR >= level: - add_log_handler(roslib.msg.Log.ERROR, _rosout_error) - if roslib.msg.Log.FATAL >= level: - add_log_handler(roslib.msg.Log.FATAL, _rosout_fatal) diff --git a/core/rospy/src/rospy/impl/simtime.py b/core/rospy/src/rospy/impl/simtime.py deleted file mode 100644 index 5acc1967..00000000 --- a/core/rospy/src/rospy/impl/simtime.py +++ /dev/null @@ -1,93 +0,0 @@ -# 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$ - -"""Internal-use: Support for simulated clock.""" - -import traceback - -import roslib.msg -import roslib.rosenv - -import rospy.core -import rospy.rostime -import rospy.topics - -# ROS clock topics and parameter config -_ROSCLOCK = '/clock' -_USE_SIMTIME = '/use_sim_time' - -# Subscriber handles for /clock and /time -_rostime_sub = None -_rosclock_sub = None - -def _is_use_simtime(): - # in order to prevent circular dependencies, this does not use the - # builtin libraries for interacting with the parameter server, at least - # until I reorganize the client vs. internal APIs better. - master_uri = roslib.rosenv.get_master_uri() - m = rospy.core.xmlrpcapi(master_uri) - code, msg, val = m.getParam(rospy.names.get_caller_id(), _USE_SIMTIME) - if code == 1 and val: - return True - return False - -from rospy.rostime import _set_rostime -def _set_rostime_clock_wrapper(time_msg): - _set_rostime(time_msg.clock) -def _set_rostime_time_wrapper(time_msg): - _set_rostime(time_msg.rostime) - -def init_simtime(): - """ - Initialize the ROS time system by connecting to the /time topic - IFF the /use_sim_time parameter is set. - """ - import logging - logger = logging.getLogger("rospy.simtime") - try: - if not _is_use_simtime(): - logger.info("%s is not set, will not subscribe to simulated time [%s] topic"%(_USE_SIMTIME, _ROSCLOCK)) - else: - global _rostime_sub, _clock_sub - if _rostime_sub is None: - logger.info("initializing %s core topic"%_ROSCLOCK) - _clock_sub = rospy.topics.Subscriber(_ROSCLOCK, roslib.msg.Clock, _set_rostime_clock_wrapper) - logger.info("connected to core topic %s"%_ROSCLOCK) - - _set_rostime(rospy.rostime.Time(0, 0)) - rospy.rostime.set_rostime_initialized(True) - return True - except Exception, e: - logger.error("Unable to initialize %s: %s\n%s", _ROSCLOCK, e, traceback.format_exc()) - return False diff --git a/core/rospy/src/rospy/impl/tcpros.py b/core/rospy/src/rospy/impl/tcpros.py deleted file mode 100644 index 2abe3686..00000000 --- a/core/rospy/src/rospy/impl/tcpros.py +++ /dev/null @@ -1,58 +0,0 @@ -# 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$ -""" -TCPROS connection protocol. - -Implements: U{http://ros.org/wiki/ROS/TCPROS} - -The rospy tcpros implementation is split into three areas: - - L{rospy.tcpros_base}: common TCPROS routines, including header and connection processing - - L{rospy.tcpros_pubsub}: Topic-specific capabilities for publishing and subscribing - - L{rospy.tcpros_service}: Service-specific capabilities -""" - -import rospy.impl.tcpros_service - -from rospy.impl.tcpros_base import init_tcpros_server, DEFAULT_BUFF_SIZE -from rospy.impl.tcpros_pubsub import TCPROSHandler - -_handler = TCPROSHandler() - -def init_tcpros(): - server = init_tcpros_server() - server.topic_connection_handler = _handler.topic_connection_handler - server.service_connection_handler = rospy.impl.tcpros_service.service_connection_handler - -def get_tcpros_handler(): - return _handler diff --git a/core/rospy/src/rospy/impl/tcpros_base.py b/core/rospy/src/rospy/impl/tcpros_base.py deleted file mode 100644 index 1ece7c7c..00000000 --- a/core/rospy/src/rospy/impl/tcpros_base.py +++ /dev/null @@ -1,657 +0,0 @@ -# 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$ - -"""Internal use: common TCPROS libraries""" - -from __future__ import with_statement - -import cStringIO -import select -import socket -import logging -import thread -import threading -import traceback - -from roslib.message import DeserializationError, Message -from roslib.network import read_ros_handshake_header, write_ros_handshake_header - -# TODO: remove * import from core -from rospy.core import * -from rospy.core import logwarn, loginfo, logerr, logdebug, rospydebug, rospyerr, rospywarn -from rospy.exceptions import ROSInternalException, TransportException, TransportTerminated, TransportInitError -from rospy.msg import deserialize_messages, serialize_message -from rospy.service import ServiceException - -from rospy.impl.transport import Transport, BIDIRECTIONAL - -logger = logging.getLogger('rospy.tcpros') - -# Receive buffer size for topics/services (in bytes) -DEFAULT_BUFF_SIZE = 65536 - -## name of our customized TCP protocol for accepting flows over server socket -TCPROS = "TCPROS" - -_PARAM_TCP_KEEPALIVE = '/tcp_keepalive' -_use_tcp_keepalive = None -def _is_use_tcp_keepalive(): - global _use_tcp_keepalive - if _use_tcp_keepalive is not None: - return _use_tcp_keepalive - # in order to prevent circular dependencies, this does not use the - # builtin libraries for interacting with the parameter server - import roslib.rosenv - m = rospy.core.xmlrpcapi(roslib.rosenv.get_master_uri()) - code, msg, val = m.getParam(rospy.names.get_caller_id(), _PARAM_TCP_KEEPALIVE) - _use_tcp_keepalive = val if code == 1 else True - return _use_tcp_keepalive - -def recv_buff(sock, b, buff_size): - """ - Read data from socket into buffer. - @param sock: socket to read from - @type sock: socket.socket - @param b: buffer to receive into - @type b: StringIO - @param buff_size: recv read size - @type buff_size: int - @return: number of bytes read - @rtype: int - """ - d = sock.recv(buff_size) - if d: - b.write(d) - return len(d) - else: #bomb out - raise TransportTerminated("unable to receive data from sender, check sender's logs for details") - -class TCPServer(object): - """ - Simple server that accepts inbound TCP/IP connections and hands - them off to a handler function. TCPServer obeys the - ROS_IP/ROS_HOSTNAME environment variables - """ - - def __init__(self, inbound_handler, port=0): - """ - Setup a server socket listening on the specified port. If the - port is omitted, will choose any open port. - @param inbound_handler: handler to invoke with - new connection - @type inbound_handler: fn(sock, addr) - @param port: port to bind to, omit/0 to bind to any - @type port: int - """ - self.port = port #will get overwritten if port=0 - self.addr = None #set at socket bind - self.is_shutdown = False - self.inbound_handler = inbound_handler - try: - self.server_sock = self._create_server_sock() - except: - self.server_sock = None - raise - - def start(self): - """Runs the run() loop in a separate thread""" - thread.start_new_thread(self.run, ()) - - def run(self): - """ - Main TCP receive loop. Should be run in a separate thread -- use start() - to do this automatically. - """ - self.is_shutdown = False - if not self.server_sock: - raise ROSInternalException("%s did not connect"%self.__class__.__name__) - while not self.is_shutdown: - try: - (client_sock, client_addr) = self.server_sock.accept() - except socket.timeout: - continue - if self.is_shutdown: - break - try: - #leave threading decisions up to inbound_handler - self.inbound_handler(client_sock, client_addr) - except socket.error, e: - if not self.is_shutdown: - traceback.print_exc() - logwarn("Failed to handle inbound connection due to socket error: %s"%e) - logdebug("TCPServer[%s] shutting down", self.port) - - - def get_full_addr(self): - """ - @return: (ip address, port) of server socket binding - @rtype: (str, int) - """ - # return roslib.network.get_host_name() instead of address so that it - # obeys ROS_IP/ROS_HOSTNAME behavior - return (roslib.network.get_host_name(), self.port) - - def _create_server_sock(self): - """ - binds the server socket. ROS_IP/ROS_HOSTNAME may restrict - binding to loopback interface. - """ - server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - server_sock.bind((roslib.network.get_bind_address(), self.port)) - (self.addr, self.port) = server_sock.getsockname() - server_sock.listen(5) - return server_sock - - def shutdown(self): - """shutdown I/O resources uses by this server""" - if not self.is_shutdown: - self.is_shutdown = True - self.server_sock.close() - -# base maintains a tcpros_server singleton that is shared between -# services and topics for inbound connections. This global is set in -# the tcprosserver constructor. Constructor is called by init_tcpros() -_tcpros_server = None - -def init_tcpros_server(): - """starts the TCPROS server socket for inbound connections""" - global _tcpros_server - if _tcpros_server is None: - _tcpros_server = TCPROSServer() - rospy.core.add_shutdown_hook(_tcpros_server.shutdown) - return _tcpros_server - -def start_tcpros_server(): - """ - start the TCPROS server if it has not started already - """ - if _tcpros_server is None: - init_tcpros_server() - return _tcpros_server.start_server() - -# provide an accessor of this so that the TCPROS Server is entirely hidden from upper layers - -def get_tcpros_server_address(): - """ - get the address of the tcpros server. - @raise Exception: if tcpros server has not been started or created - """ - return _tcpros_server.get_address() - -def _error_connection_handler(sock, client_addr, header): - """ - utility handler that does nothing more than provide a rejection header - @param sock: socket connection - @type sock: socket.socket - @param client_addr: client address - @type client_addr: str - @param header: request header - @type header: dict - """ - return {'error': "unhandled connection"} - -class TCPROSServer(object): - """ - ROS Protocol handler for TCPROS. Accepts both TCPROS topic - connections as well as ROS service connections over TCP. TCP server - socket is run once start_server() is called -- this is implicitly - called during init_publisher(). - """ - - def __init__(self): - """ctor.""" - self.tcp_ros_server = None #: server for receiving tcp conn - self.lock = threading.Lock() - # should be set to fn(sock, client_addr, header) for topic connections - self.topic_connection_handler = _error_connection_handler - # should be set to fn(sock, client_addr, header) for service connections - self.service_connection_handler = _error_connection_handler - - def start_server(self, port=0): - """ - Starts the TCP socket server if one is not already running - @param port: port number to bind to (default 0/any) - @type port: int - """ - if self.tcp_ros_server: - return - with self.lock: - try: - if not self.tcp_ros_server: - self.tcp_ros_server = TCPServer(self._tcp_server_callback, port) - self.tcp_ros_server.start() - except Exception, e: - self.tcp_ros_server = None - logerr("unable to start TCPROS server: %s\n%s"%(e, traceback.format_exc())) - return 0, "unable to establish TCPROS server: %s"%e, [] - - def get_address(self): - """ - @return: address and port of TCP server socket for accepting - inbound connections - @rtype: str, int - """ - if self.tcp_ros_server is not None: - return self.tcp_ros_server.get_full_addr() - return None, None - - def shutdown(self, reason=''): - """stops the TCP/IP server responsible for receiving inbound connections""" - if self.tcp_ros_server: - self.tcp_ros_server.shutdown() - - def _tcp_server_callback(self, sock, client_addr): - """ - TCPServer callback: detects incoming topic or service connection and passes connection accordingly - - @param sock: socket connection - @type sock: socket.socket - @param client_addr: client address - @type client_addr: (str, int) - @raise TransportInitError: If transport cannot be succesfully initialized - """ - #TODOXXX:rewrite this logic so it is possible to create TCPROSTransport object first, set its protocol, - #and then use that to do the writing - try: - buff_size = 4096 # size of read buffer - header = read_ros_handshake_header(sock, cStringIO.StringIO(), buff_size) - if 'topic' in header: - err_msg = self.topic_connection_handler(sock, client_addr, header) - elif 'service' in header: - err_msg = self.service_connection_handler(sock, client_addr, header) - else: - err_msg = 'no topic or service name detected' - if err_msg: - # shutdown race condition: nodes that come up and down quickly can receive connections during teardown - if not rospy.core.is_shutdown(): - write_ros_handshake_header(sock, {'error' : err_msg}) - raise TransportInitError("Could not process inbound connection: "+err_msg+str(header)) - else: - write_ros_handshake_header(sock, {'error' : 'node shutting down'}) - return - except rospy.exceptions.TransportInitError, e: - logwarn(str(e)) - if sock is not None: - sock.close() - except Exception, e: - # collect stack trace separately in local log file - logwarn("Inbound TCP/IP connection failed: %s", e) - rospyerr("Inbound TCP/IP connection failed:\n%s", traceback.format_exc(e)) - if sock is not None: - sock.close() - -class TCPROSTransportProtocol(object): - """ - Abstraction of TCPROS connections. Implementations Services/Publishers/Subscribers must implement this - protocol, which defines how messages are deserialized from an inbound connection (read_messages()) as - well as which fields to send when creating a new connection (get_header_fields()). - """ - - def __init__(self, resolved_name, recv_data_class, queue_size=None, buff_size=DEFAULT_BUFF_SIZE): - """ - ctor - @param resolved_name: resolved service or topic name - @type resolved_name: str - @param recv_data_class: message class for deserializing inbound messages - @type recv_data_class: Class - @param queue_size: maximum number of inbound messages to maintain - @type queue_size: int - @param buff_size: receive buffer size (in bytes) for reading from the connection. - @type buff_size: int - """ - if recv_data_class and not issubclass(recv_data_class, Message): - raise TransportInitError("Unable to initialize transport: data class is not a message data class") - self.resolved_name = resolved_name - self.recv_data_class = recv_data_class - self.queue_size = queue_size - self.buff_size = buff_size - self.direction = BIDIRECTIONAL - - - def read_messages(self, b, msg_queue, sock): - """ - @param b StringIO: read buffer - @param msg_queue [Message]: queue of deserialized messages - @type msg_queue: [Message] - @param sock socket: protocol can optionally read more data from - the socket, but in most cases the required data will already be - in b - """ - # default implementation - deserialize_messages(b, msg_queue, self.recv_data_class, queue_size=self.queue_size) - - def get_header_fields(self): - """ - Header fields that should be sent over the connection. The header fields - are protocol specific (i.e. service vs. topic, publisher vs. subscriber). - @return: {str : str}: header fields to send over connection - @rtype: dict - """ - return {} - -# TODO: this still isn't as clean and seamless as I want it to -# be. This code came from the merger of publisher, subscriber, and -# service code into a common TCPROS transport class. The transport is -# customized by a 'protocol' class, which is how the different -# pub/sub/service behaviors are achieved. More behavior needs to be -# transferred from the transport class into the protocol class, -# e.g. deserialization as the state each maintains is somewhat -# duplicative. I would also come up with a better name than -# protocol. - -class TCPROSTransport(Transport): - """ - Generic implementation of TCPROS exchange routines for both topics and services - """ - transport_type = 'TCPROS' - - def __init__(self, protocol, name, header=None): - """ - ctor - @param name str: identifier - @param protocol TCPROSTransportProtocol protocol implementation - @param header dict: (optional) handshake header if transport handshake header was - already read off of transport. - @raise TransportInitError if transport cannot be initialized according to arguments - """ - super(TCPROSTransport, self).__init__(protocol.direction, name=name) - if not name: - raise TransportInitError("Unable to initialize transport: name is not set") - - self.protocol = protocol - - self.socket = None - self.endpoint_id = 'unknown' - self.read_buff = cStringIO.StringIO() - self.write_buff = cStringIO.StringIO() - - self.header = header - - # #1852 have to hold onto latched messages on subscriber side - self.is_latched = False - self.latch = None - - # these fields are actually set by the remote - # publisher/service. they are set for tools that connect - # without knowing the actual field name - self.md5sum = None - self.type = None - - def set_socket(self, sock, endpoint_id): - """ - Set the socket for this transport - @param sock: socket - @type sock: socket.socket - @param endpoint_id: identifier for connection endpoint - @type endpoint_id: str - @raise TransportInitError: if socket has already been set - """ - if self.socket is not None: - raise TransportInitError("socket already initialized") - self.socket = sock - self.endpoint_id = endpoint_id - - def connect(self, dest_addr, dest_port, endpoint_id, timeout=None): - """ - Establish TCP connection to the specified - address/port. connect() always calls L{write_header()} and - L{read_header()} after the connection is made - @param dest_addr: destination IP address - @type dest_addr: str - @param dest_port: destination port - @type dest_port: int - @param endpoint_id: string identifier for connection (for statistics) - @type endpoint_id: str - @param timeout: (optional keyword) timeout in seconds - @type timeout: float - @raise TransportInitError: if unable to create connection - """ - try: - self.endpoint_id = endpoint_id - - s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - if _is_use_tcp_keepalive(): - # OSX (among others) does not define these options - if hasattr(socket, 'TCP_KEEPCNT') and \ - hasattr(socket, 'TCP_KEEPIDLE') and \ - hasattr(socket, 'TCP_KEEPINTVL'): - # turn on KEEPALIVE - s.setsockopt(socket.SOL_SOCKET, socket.SO_KEEPALIVE, 1) - # - # keepalive failures before actual connection failure - s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPCNT, 9) - # - timeout before starting KEEPALIVE process - s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPIDLE, 60) - # - interval to send KEEPALIVE after IDLE timeout - s.setsockopt(socket.SOL_TCP, socket.TCP_KEEPINTVL, 10) - if timeout is not None: - s.settimeout(timeout) - self.socket = s - self.socket.connect((dest_addr, dest_port)) - self.write_header() - self.read_header() - except TransportInitError, tie: - rospyerr("Unable to initiate TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc())) - raise - except Exception, e: - self.done = True - if self.socket: - self.socket.close() - self.socket = None - - #logerr("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, str(e))) - rospywarn("Unknown error initiating TCP/IP socket to %s:%s (%s): %s"%(dest_addr, dest_port, endpoint_id, traceback.format_exc())) - raise TransportInitError(str(e)) #re-raise i/o error - - def _validate_header(self, header): - """ - Validate header and initialize fields accordingly - @param header: header fields from publisher - @type header: dict - @raise TransportInitError: if header fails to validate - """ - self.header = header - if 'error' in header: - raise TransportInitError("remote error reported: %s"%header['error']) - for required in ['md5sum', 'type']: - if not required in header: - raise TransportInitError("header missing required field [%s]"%required) - self.md5sum = header['md5sum'] - self.type = header['type'] - if header.get('latching', '0') == '1': - self.is_latched = True - - def write_header(self): - """Writes the TCPROS header to the active connection.""" - sock = self.socket - # socket may still be getting spun up, so wait for it to be writable - fileno = sock.fileno() - ready = None - while not ready: - _, ready, _ = select.select([], [fileno], []) - logger.debug("[%s]: writing header", self.name) - sock.setblocking(1) - self.stat_bytes += write_ros_handshake_header(sock, self.protocol.get_header_fields()) - - def read_header(self): - """ - Read TCPROS header from active socket - @raise TransportInitError if header fails to validate - """ - self.socket.setblocking(1) - self._validate_header(read_ros_handshake_header(self.socket, self.read_buff, self.protocol.buff_size)) - - def send_message(self, msg, seq): - """ - Convenience routine for services to send a message across a - particular connection. NOTE: write_data is much more efficient - if same message is being sent to multiple connections. Not - threadsafe. - @param msg: message to send - @type msg: Msg - @param seq: sequence number for message - @type seq: int - @raise TransportException: if error occurred sending message - """ - # this will call write_data(), so no need to keep track of stats - serialize_message(self.write_buff, seq, msg) - self.write_data(self.write_buff.getvalue()) - self.write_buff.truncate(0) - - def write_data(self, data): - """ - Write raw data to transport - @raise TransportInitialiationError: could not be initialized - @raise TransportTerminated: no longer open for publishing - """ - if not self.socket: - raise TransportInitError("TCPROS transport was not successfully initialized") - if self.done: - raise TransportTerminated("connection closed") - try: - #TODO: get rid of sendalls and replace with async-style publishing - self.socket.sendall(data) - self.stat_bytes += len(data) - self.stat_num_msg += 1 - except IOError, (errno, msg): - #for now, just document common errno's in code - if errno == 32: #broken pipe - logdebug("ERROR: Broken Pipe") - self.close() - raise TransportTerminated(str(errno)+msg) - raise #re-raise - except socket.error, (errno, msg): - #for now, just document common errno's in code - if errno == 32: #broken pipe - logdebug("[%s]: Closing connection [%s] due to broken pipe", self.name, self.endpoint_id) - self.close() - raise TransportTerminated(msg) - elif errno == 104: #connection reset by peer - logdebug("[%s]: Peer [%s] has closed connection", self.name, self.endpoint_id) - self.close() - raise TransportTerminated(msg) - else: - rospydebug("unknown socket error writing data: %s",traceback.format_exc()) - logdebug("[%s]: closing connection [%s] due to unknown socket error: %s", self.name, self.endpoint_id, msg) - self.close() - raise TransportTerminated(str(errno)+' '+msg) - except: - #TODO: try to figure out common errors here - raise - return True - - def receive_once(self): - """ - block until messages are read off of socket - @return: list of newly received messages - @rtype: [Msg] - @raise TransportException: if unable to receive message due to error - """ - sock = self.socket - if sock is None: - raise TransportException("connection not initialized") - b = self.read_buff - msg_queue = [] - p = self.protocol - try: - sock.setblocking(1) - while not msg_queue and not self.done and not is_shutdown(): - if b.tell() >= 4: - p.read_messages(b, msg_queue, sock) - if not msg_queue: - recv_buff(sock, b, p.buff_size) - self.stat_num_msg += len(msg_queue) #STATS - # set the _connection_header field - for m in msg_queue: - m._connection_header = self.header - - # #1852: keep track of last latched message - if self.is_latched and msg_queue: - self.latch = msg_queue[-1] - - return msg_queue - - except DeserializationError, e: - rospyerr(traceback.format_exc()) - raise TransportException("receive_once[%s]: DeserializationError %s"%(self.name, str(e))) - except TransportTerminated, e: - raise #reraise - except ServiceException, e: - raise - except Exception, e: - rospyerr(traceback.format_exc()) - raise TransportException("receive_once[%s]: unexpected error %s"%(self.name, str(e))) - return retval - - def receive_loop(self, msgs_callback): - """ - Receive messages until shutdown - @param msgs_callback: callback to invoke for new messages received - @type msgs_callback: fn([msg]) - """ - # - use assert here as this would be an internal error, aka bug - logger.debug("receive_loop for [%s]", self.name) - try: - try: - while not self.done and not is_shutdown(): - msgs = self.receive_once() - if not self.done and not is_shutdown(): - msgs_callback(msgs) - - rospydebug("receive_loop[%s]: done condition met, exited loop"%self.name) - except DeserializationError, e: - logerr("[%s] error deserializing incoming request: %s"%self.name, str(e)) - rospyerr("[%s] error deserializing incoming request: %s"%self.name, traceback.format_exc()) - except: - # in many cases this will be a normal hangup, but log internally - try: - #1467 sometimes we get exceptions due to - #interpreter shutdown, so blanket ignore those if - #the reporting fails - rospydebug("exception in receive loop for [%s], may be normal. Exception is %s",self.name, traceback.format_exc()) - except: pass - finally: - if not self.done: - self.close() - - def close(self): - """close i/o and release resources""" - self.done = True - try: - if self.socket is not None: - self.socket.close() - finally: - self.socket = self.read_buff = self.write_buff = self.protocol = None - super(TCPROSTransport, self).close() - diff --git a/core/rospy/src/rospy/impl/tcpros_pubsub.py b/core/rospy/src/rospy/impl/tcpros_pubsub.py deleted file mode 100644 index e10b5366..00000000 --- a/core/rospy/src/rospy/impl/tcpros_pubsub.py +++ /dev/null @@ -1,317 +0,0 @@ -# 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$ - -"""Internal use: Topic-specific extensions for TCPROS support""" - -import socket -import threading - -from rospy.core import logwarn, logerr, logdebug -import rospy.exceptions -import rospy.names - -import rospy.impl.registration -import rospy.impl.transport - -from rospy.impl.tcpros_base import TCPROSTransport, TCPROSTransportProtocol, \ - get_tcpros_server_address, start_tcpros_server,\ - DEFAULT_BUFF_SIZE, TCPROS - -class TCPROSSub(TCPROSTransportProtocol): - """ - Subscription transport implementation for receiving topic data via - peer-to-peer TCP/IP sockets - """ - - def __init__(self, resolved_name, recv_data_class, queue_size=None, \ - buff_size=DEFAULT_BUFF_SIZE, tcp_nodelay=False): - """ - ctor. - - @param resolved_name: resolved subscription name - @type resolved_name: str - - @param recv_data_class: class to instantiate to receive - messages - @type recv_data_class: L{rospy.Message} - - @param queue_size: maximum number of messages to - deserialize from newly read data off socket - @type queue_size: int - - @param buff_size: recv buffer size - @type buff_size: int - - @param tcp_nodelay: If True, request TCP_NODELAY from publisher - @type tcp_nodelay: bool - """ - super(TCPROSSub, self).__init__(resolved_name, recv_data_class, queue_size, buff_size) - self.direction = rospy.impl.transport.INBOUND - self.tcp_nodelay = tcp_nodelay - - def get_header_fields(self): - """ - @return: dictionary of subscriber fields - @rtype: dict - """ - return {'topic': self.resolved_name, - 'message_definition': self.recv_data_class._full_text, - 'tcp_nodelay': '1' if self.tcp_nodelay else '0', - 'md5sum': self.recv_data_class._md5sum, - 'type': self.recv_data_class._type, - 'callerid': rospy.names.get_caller_id()} - -# Separate method for easier testing -def _configure_pub_socket(sock, is_tcp_nodelay): - """ - Configure socket options on a new publisher socket. - @param sock: socket.socket - @type sock: socket.socket - @param is_tcp_nodelay: if True, TCP_NODELAY will be set on outgoing socket if available - @param is_tcp_nodelay: bool - """ - # #956: low latency, TCP_NODELAY support - if is_tcp_nodelay: - if hasattr(socket, 'TCP_NODELAY'): - sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) - else: - logwarn("WARNING: cannot enable TCP_NODELAY as its not supported on this platform") - -#TODO:POLLING: TCPROSPub currently doesn't actually do anything -- not until polling is implemented - -class TCPROSPub(TCPROSTransportProtocol): - """ - Publisher transport implementation for publishing topic data via - peer-to-peer TCP/IP sockets. - """ - - def __init__(self, resolved_name, pub_data_class, is_latch=False, headers=None): - """ - ctor. - @param resolved_name: resolved topic name - @type resolved_name: str - @param pub_data_class: class to instance to receive messages - @type pub_data_class: L{rospy.Message} class - @param is_latch: If True, Publisher is latching - @type is_latch: bool - """ - # very small buffer size for publishers as the messages they receive are very small - super(TCPROSPub, self).__init__(resolved_name, None, queue_size=None, buff_size=128) - self.pub_data_class = pub_data_class - self.direction = rospy.impl.transport.OUTBOUND - self.is_latch = is_latch - self.headers = headers if headers else {} - - def get_header_fields(self): - base = {'topic': self.resolved_name, - 'type': self.pub_data_class._type, - 'latching': '1' if self.is_latch else '0', - 'message_definition': self.pub_data_class._full_text, - 'md5sum': self.pub_data_class._md5sum, - 'callerid': rospy.names.get_caller_id() } - - # this implementation allows the user to override builtin - # fields. this could potentially enable some interesting - # features... or it could be really bad. - if self.headers: - base.update(self.headers) - return base - -class TCPROSHandler(rospy.impl.transport.ProtocolHandler): - """ - ROS Protocol handler for TCPROS. Accepts both TCPROS topic - connections as well as ROS service connections over TCP. TCP server - socket is run once start_server() is called -- this is implicitly - called during init_publisher(). - """ - - def __init__(self): - """ctor""" - self.tcp_nodelay_map = {} # { topic : tcp_nodelay} - - def set_tcp_nodelay(self, resolved_name, tcp_nodelay): - """ - @param resolved_name: resolved topic name - @type resolved_name: str - - @param tcp_nodelay: If True, sets TCP_NODELAY on publisher's - socket (disables Nagle algorithm). This results in lower - latency publishing at the cost of efficiency. - @type tcp_nodelay: bool - """ - self.tcp_nodelay_map[resolved_name] = tcp_nodelay - - def shutdown(self): - """ - stops the TCP/IP server responsible for receiving inbound connections - """ - pass - - def create_transport(self, resolved_name, pub_uri, protocol_params): - """ - Connect to topic resolved_name on Publisher pub_uri using TCPROS. - @param resolved_name str: resolved topic name - @type resolved_name: str - @param pub_uri: XML-RPC URI of publisher - @type pub_uri: str - @param protocol_params: protocol parameters to use for connecting - @type protocol_params: [XmlRpcLegal] - @return: code, message, debug - @rtype: (int, str, int) - """ - - #Validate protocol params = [TCPROS, address, port] - if type(protocol_params) != list or len(protocol_params) != 3: - return 0, "ERROR: invalid TCPROS parameters", 0 - if protocol_params[0] != TCPROS: - return 0, "INTERNAL ERROR: protocol id is not TCPROS: %s"%id, 0 - id, dest_addr, dest_port = protocol_params - - sub = rospy.impl.registration.get_topic_manager().get_subscriber_impl(resolved_name) - - #Create connection - try: - protocol = TCPROSSub(resolved_name, sub.data_class, \ - queue_size=sub.queue_size, buff_size=sub.buff_size, - tcp_nodelay=sub.tcp_nodelay) - conn = TCPROSTransport(protocol, resolved_name) - # timeout is really generous. for now just choosing one that is large but not infinite - conn.connect(dest_addr, dest_port, pub_uri, timeout=60.) - t = threading.Thread(name=resolved_name, target=conn.receive_loop, args=(sub.receive_callback,)) - # don't enable this just yet, need to work on this logic - #rospy.core._add_shutdown_thread(t) - t.start() - except rospy.exceptions.TransportInitError, e: - rospyerr("unable to create subscriber transport: %s", e) - return 0, "Internal error creating inbound TCP connection for [%s]: %s"%(resolved_name, e), -1 - - # Attach connection to _SubscriberImpl - if sub.add_connection(conn): #pass tcp connection to handler - return 1, "Connected topic[%s]. Transport impl[%s]"%(resolved_name, conn.__class__.__name__), dest_port - else: - conn.close() - return 0, "ERROR: Race condition failure: duplicate topic subscriber [%s] was created"%(resolved_name), 0 - - def supports(self, protocol): - """ - @param protocol: name of protocol - @type protocol: str - @return: True if protocol is supported - @rtype: bool - """ - return protocol == TCPROS - - def get_supported(self): - """ - Get supported protocols - """ - return [[TCPROS]] - - def init_publisher(self, resolved_name, protocol): - """ - Initialize this node to receive an inbound TCP connection, - i.e. startup a TCP server if one is not already running. - - @param resolved_name: topic name - @type resolved__name: str - - @param protocol: negotiated protocol - parameters. protocol[0] must be the string 'TCPROS' - @type protocol: [str, value*] - @return: (code, msg, [TCPROS, addr, port]) - @rtype: (int, str, list) - """ - if protocol[0] != TCPROS: - return 0, "Internal error: protocol does not match TCPROS: %s"%protocol, [] - start_tcpros_server() - addr, port = get_tcpros_server_address() - return 1, "ready on %s:%s"%(addr, port), [TCPROS, addr, port] - - def topic_connection_handler(self, sock, client_addr, header): - """ - Process incoming topic connection. Reads in topic name from - handshake and creates the appropriate L{TCPROSPub} handler for the - connection. - @param sock: socket connection - @type sock: socket.socket - @param client_addr: client address - @type client_addr: (str, int) - @param header: key/value pairs from handshake header - @type header: dict - @return: error string or None - @rtype: str - """ - for required in ['topic', 'md5sum', 'callerid']: - if not required in header: - return "Missing required '%s' field"%required - else: - resolved_topic_name = header['topic'] - md5sum = header['md5sum'] - tm = rospy.impl.registration.get_topic_manager() - topic = tm.get_publisher_impl(resolved_topic_name) - if not topic: - return "[%s] is not a publisher of [%s]. Topics are %s"%(rospy.names.get_caller_id(), resolved_topic_name, tm.get_publications()) - elif md5sum != rospy.names.TOPIC_ANYTYPE and md5sum != topic.data_class._md5sum: - - actual_type = topic.data_class._type - - # check to see if subscriber sent 'type' header. If they did, check that - # types are same first as this provides a better debugging message - if 'type' in header: - requested_type = header['type'] - if requested_type != actual_type: - return "topic types do not match: [%s] vs. [%s]"%(requested_type, actual_type) - else: - # defaults to actual type - requested_type = actual_type - - return "Client [%s] wants topic [%s] to have datatype/md5sum [%s/%s], but our version has [%s/%s] Dropping connection."%(header['callerid'], resolved_topic_name, requested_type, md5sum, actual_type, topic.data_class._md5sum) - - else: - #TODO:POLLING if polling header is present, have to spin up receive loop as well - - # #1334: tcp_nodelay support from subscriber option - if 'tcp_nodelay' in header: - tcp_nodelay = True if header['tcp_nodelay'].strip() == '1' else False - else: - tcp_nodelay = self.tcp_nodelay_map.get(resolved_topic_name, False) - - _configure_pub_socket(sock, tcp_nodelay) - protocol = TCPROSPub(resolved_topic_name, topic.data_class, is_latch=topic.is_latch, headers=topic.headers) - transport = TCPROSTransport(protocol, resolved_topic_name) - transport.set_socket(sock, header['callerid']) - transport.write_header() - topic.add_connection(transport) - - diff --git a/core/rospy/src/rospy/impl/tcpros_service.py b/core/rospy/src/rospy/impl/tcpros_service.py deleted file mode 100644 index a2c9ac29..00000000 --- a/core/rospy/src/rospy/impl/tcpros_service.py +++ /dev/null @@ -1,682 +0,0 @@ -# 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$ - -"""Internal use: Service-specific extensions for TCPROS support""" - -import cStringIO -import socket -import struct -import sys -import logging -import thread -import time -import traceback - -import roslib.scriptutil - -from rospy.exceptions import TransportInitError, TransportTerminated, ROSException, ROSInterruptException -from rospy.service import _Service, ServiceException - -from rospy.impl.registration import get_service_manager -from rospy.impl.tcpros_base import TCPROSTransport, TCPROSTransportProtocol, \ - get_tcpros_server_address, start_tcpros_server, recv_buff, \ - DEFAULT_BUFF_SIZE - -from rospy.core import logwarn, loginfo, logerr, logdebug -import rospy.core -import rospy.msg -import rospy.names - -import rospy.impl.validators - -logger = logging.getLogger('rospy.service') - -######################################################### -# Service helpers - -def wait_for_service(service, timeout=None): - """ - Blocks until service is available. Use this in - initialization code if your program depends on a - service already running. - @param service: name of service - @type service: str - @param timeout: timeout time in seconds, None for no - timeout. NOTE: timeout=0 is invalid as wait_for_service actually - contacts the service, so non-blocking behavior is not - possible. For timeout=0 uses cases, just call the service without - waiting. - @type timeout: double - @raise ROSException: if specified timeout is exceeded - @raise ROSInterruptException: if shutdown interrupts wait - """ - def contact_service(resolved_name, timeout=10.0): - code, _, uri = master.lookupService(rospy.names.get_caller_id(), resolved_name) - if False and code == 1: - return True - elif True and code == 1: - # disabling for now as this is causing socket.error 22 "Invalid argument" on OS X - addr = rospy.core.parse_rosrpc_uri(uri) - s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - try: - # we always want to timeout just in case we're connecting - # to a down service. - s.settimeout(timeout) - s.connect(addr) - h = { 'probe' : '1', 'md5sum' : '*', - 'callerid' : rospy.core.get_caller_id(), - 'service': resolved_name } - roslib.network.write_ros_handshake_header(s, h) - return True - finally: - if s is not None: - s.close() - if timeout == 0.: - raise ValueError("timeout must be non-zero") - resolved_name = rospy.names.resolve_name(service) - master = roslib.scriptutil.get_master() - first = False - if timeout: - timeout_t = time.time() + timeout - while not rospy.core.is_shutdown() and time.time() < timeout_t: - try: - if contact_service(resolved_name, timeout_t-time.time()): - return - time.sleep(0.3) - except KeyboardInterrupt: - # re-raise - rospy.core.logdebug("wait_for_service: received keyboard interrupt, assuming signals disabled and re-raising") - raise - except: # service not actually up - if first: - first = False - rospy.core.logerr("wait_for_service(%s): failed to contact [%s], will keep trying"%(resolved_name, uri)) - if rospy.core.is_shutdown(): - raise ROSInterruptException("rospy shutdown") - else: - raise ROSException("timeout exceeded while waiting for service %s"%resolved_name) - else: - while not rospy.core.is_shutdown(): - try: - if contact_service(resolved_name): - return - time.sleep(0.3) - except KeyboardInterrupt: - # re-raise - rospy.core.logdebug("wait_for_service: received keyboard interrupt, assuming signals disabled and re-raising") - raise - except: # service not actually up - if first: - first = False - rospy.core.logerr("wait_for_service(%s): failed to contact [%s], will keep trying"%(resolved_name, uri)) - if rospy.core.is_shutdown(): - raise ROSInterruptException("rospy shutdown") - - -def convert_return_to_response(response, response_class): - """ - Convert return value of function to response instance. The - rules/precedence for this are: - - 1. If the return type is the same as the response type, no conversion - is done. - - 2. If the return type is a dictionary, it is used as a keyword-style - initialization for a new response instance. - - 3. If the return type is *not* a list type, it is passed in as a single arg - to a new response instance. - - 4. If the return type is a list/tuple type, it is used as a args-style - initialization for a new response instance. - """ - - # use this declared ROS type check instead of a direct instance - # check, which allows us to play tricks with serialization and - # deserialization - if isinstance(response, roslib.message.Message) and response._type == response_class._type: - #if isinstance(response, response_class): - return response - elif type(response) == dict: - # kwds response - try: - return response_class(**response) - except AttributeError, e: - raise ServiceException("handler returned invalid value: %s"%str(e)) - elif response == None: - raise ServiceException("service handler returned None") - elif type(response) not in [list, tuple]: - # single, non-list arg - try: - return response_class(response) - except TypeError, e: - raise ServiceException("handler returned invalid value: %s"%str(e)) - else: - # user returned a list, which has some ambiguous cases. Our resolution is that - # all list/tuples are converted to *args - try: - return response_class(*response) - except TypeError, e: - raise ServiceException("handler returned wrong number of values: %s"%str(e)) - -def service_connection_handler(sock, client_addr, header): - """ - Process incoming service connection. For use with - TCPROSServer. Reads in service name from handshake and creates the - appropriate service handler for the connection. - @param sock: socket connection - @type sock: socket - @param client_addr: client address - @type client_addr: (str, int) - @param header: key/value pairs from handshake header - @type header: dict - @return: error string or None - @rtype: str - """ - for required in ['service', 'md5sum', 'callerid']: - if not required in header: - return "Missing required '%s' field"%required - else: - logger.debug("connection from %s:%s", client_addr[0], client_addr[1]) - service_name = header['service'] - - #TODO: make service manager configurable. I think the right - #thing to do is to make these singletons private members of a - #Node instance and enable rospy to have multiple node - #instances. - - sm = get_service_manager() - md5sum = header['md5sum'] - service = sm.get_service(service_name) - if not service: - return "[%s] is not a provider of [%s]"%(rospy.names.get_caller_id(), service_name) - elif md5sum != rospy.names.SERVICE_ANYTYPE and md5sum != service.service_class._md5sum: - return "request from [%s]: md5sums do not match: [%s] vs. [%s]"%(header['callerid'], md5sum, service.service_class._md5sum) - else: - transport = TCPROSTransport(service.protocol, service_name, header=header) - transport.set_socket(sock, header['callerid']) - transport.write_header() - # using threadpool reduced performance by an order of - # magnitude, need to investigate better - thread.start_new_thread(service.handle, (transport, header)) - - -class TCPService(TCPROSTransportProtocol): - """ - Protocol implementation for Services over TCPROS - """ - - def __init__(self, resolved_name, service_class, buff_size=DEFAULT_BUFF_SIZE): - """ - ctor. - @param resolved_name: name of service - @type resolved_name: str - @param service_class: Service data type class - @type service_class: Service - @param buff_size int: size of buffer (bytes) to use for reading incoming requests. - @type buff_size: int - """ - super(TCPService, self).__init__(resolved_name, service_class._request_class, buff_size=buff_size) - self.service_class = service_class - - def get_header_fields(self): - """ - Protocol API - @return: header fields - @rtype: dict - """ - return {'service': self.resolved_name, 'type': self.service_class._type, - 'md5sum': self.service_class._md5sum, 'callerid': rospy.names.get_caller_id() } - -class TCPROSServiceClient(TCPROSTransportProtocol): - """Protocol Implementation for Service clients over TCPROS""" - - def __init__(self, resolved_name, service_class, headers=None, buff_size=DEFAULT_BUFF_SIZE): - """ - ctor. - @param resolved_name: resolved service name - @type resolved_name: str - @param service_class: Service data type class - @type service_class: Service - @param headers: identifier for Service session - @type headers: dict - @param buff_size: size of buffer (bytes) for reading responses from Service. - @type buff_size: int - """ - super(TCPROSServiceClient, self).__init__(resolved_name, service_class._response_class) - self.service_class = service_class - self.headers = headers or {} - self.buff_size = buff_size - - def get_header_fields(self): - """ - TCPROSTransportProtocol API - """ - headers = {'service': self.resolved_name, 'md5sum': self.service_class._md5sum, - 'callerid': rospy.names.get_caller_id()} - # The current implementation allows user-supplied headers to - # override protocol-specific headers. We may want to - # eliminate this in the future if it is abused too severely. - for k, v in self.headers.iteritems(): - headers[k] = v - return headers - - def _read_ok_byte(self, b, sock): - """ - Utility for reading the OK-byte/error-message header preceding each message. - @param sock: socket connection. Will be read from if OK byte is - false and error message needs to be read - @type sock: socket.socket - @param b: buffer to read from - @type b: StringIO - """ - if b.tell() == 0: - return - pos = b.tell() - b.seek(0) - ok = struct.unpack(' 1: - raise ServiceException("service [%s] returned multiple responses: %s"%(self.resolved_name, len(responses))) - except rospy.exceptions.TransportException, e: - # convert lower-level exception to exposed type - if rospy.core.is_shutdown(): - raise rospy.exceptions.ROSInterruptException("node shutdown interrupted service call") - else: - raise ServiceException("transport error completing service call: %s"%(str(e))) - finally: - if not self.persistent: - transport.close() - self.transport = None - return responses[0] - - - def close(self): - """Close this ServiceProxy. This only has an effect on persistent ServiceProxy instances.""" - if self.transport is not None: - self.transport.close() - -class ServiceImpl(_Service): - """ - Implementation of ROS Service. This intermediary class allows for more configuration of behavior than the Service class. - """ - - def __init__(self, name, service_class, handler, buff_size=DEFAULT_BUFF_SIZE): - super(ServiceImpl, self).__init__(name, service_class) - - if not name or not isinstance(name, basestring): - raise ValueError("service name is not a non-empty string") - # #2202 - if not roslib.names.is_legal_name(name): - import warnings - warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%name, stacklevel=2) - - - self.handler = handler - self.registered = False - self.seq = 0 - self.done = False - self.buff_size=buff_size - - start_tcpros_server() #lazy-init the tcprosserver - host, port = get_tcpros_server_address() - self.uri = '%s%s:%s'%(rospy.core.ROSRPC, host, port) - logdebug("... service URL is %s"%self.uri) - - self.protocol = TCPService(self.resolved_name, service_class, self.buff_size) - - logdebug("[%s]: new Service instance"%self.resolved_name) - - # TODO: should consider renaming to unregister - - def shutdown(self, reason=''): - """ - Stop this service - @param reason: human-readable shutdown reason - @type reason: str - """ - self.done = True - logdebug('[%s].shutdown: reason [%s]'%(self.resolved_name, reason)) - try: - #TODO: make service manager configurable - get_service_manager().unregister(self.resolved_name, self) - except Exception, e: - logerr("Unable to unregister with master: "+traceback.format_exc()) - raise ServiceException("Unable to connect to master: %s"%e) - - def spin(self): - """ - Let service run and take over thread until service or node - shutdown. Use this method to keep your scripts from exiting - execution. - """ - try: - while not rospy.core.is_shutdown() and not self.done: - time.sleep(0.5) - except KeyboardInterrupt: - logdebug("keyboard interrupt, shutting down") - - def _write_service_error(self, transport, err_msg): - """ - Send error message to client - @param transport: transport connection to client - @type transport: Transport - @param err_msg: error message to send to client - @type err_msg: str - """ - transport.write_data(struct.pack('resp - @param buff_size: size of buffer for reading incoming requests. Should be at least size of request message - @type buff_size: int - """ - super(Service, self).__init__(name, service_class, handler, buff_size) - - #TODO: make service manager configurable - get_service_manager().register(self.resolved_name, self) diff --git a/core/rospy/src/rospy/impl/transport.py b/core/rospy/src/rospy/impl/transport.py deleted file mode 100644 index 672b6132..00000000 --- a/core/rospy/src/rospy/impl/transport.py +++ /dev/null @@ -1,190 +0,0 @@ -# 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$ -""" -Base classes for rospy transports - -These are the base underlying transport implementations, i.e. -TCP/IP connections, etc... - -For topic implementations, see L{topics} -""" - -import threading - -# we need ids for the transports so we can send the IDs instead of -# full connection details -_transport_id = 0 -_id_lock = threading.Lock() -def _nextId(): - global _transport_id - try: - _id_lock.acquire() - _transport_id += 1 - return _transport_id - finally: - _id_lock.release() - -INBOUND = 'i' -OUTBOUND = 'o' -BIDIRECTIONAL = 'b' - -## Base API of Transport implementations -class Transport(object): - transport_type = 'UNKNOWN' - - ## @param self - ## @param direction str: INBOUND | OUTBOUND | BIDIRECTIONAL - ## @param name str - def __init__(self, direction, name='unnamed'): - self.name = name - self.direction = direction - self.done = False - self.cleanup_cb = None - self.endpoint_id = '' - - #STATS - self.id = _nextId() - self.stat_bytes = 0 - # Number of messages that have passed through this transport - self.stat_num_msg = 0 - - ## callback function to invoke when this connection is - ## closed. Function will be passed this transport as an argument. - ## @param self - ## @param cleanup_callback fn(Transport): callback for when connection is closed - def set_cleanup_callback(self, cleanup_callback): - self.cleanup_cb = cleanup_callback - - ## terminate i/o. Leaf subclasses should override and call this implementation - ## @param self - def close(self): - self.done = True - if self.cleanup_cb: - self.cleanup_cb(self) - - ## Write raw data to transport - ## @throws TransportInitialiationError could not be initialized - ## @throws TransportTerminated no longer open for publishing - def write_data(self, data): - raise Exception("not implemented") - -## Shell class to hold stats about transport that is being killed off. -## This allows the information to stick around but the original Tranport to be gc'd -class DeadTransport(Transport): - - ## @param self - ## @param transport str: transport name - def __init__(self, transport): - super(DeadTransport, self).__init__( - transport.direction, transport.name) - self.transport_type = transport.transport_type #class property - self.id = transport.id - self.stat_bytes = transport.stat_bytes - self.stat_num_msg = transport.stat_num_msg - self.done = True - self.endpoint_id = transport.endpoint_id - -## ProtocolHandler interface: implements topic communication for a -## particular protocol(s). In order to understand the methods of this -## API, it is important to understand how topic communication is -## established: -## -## When a subscriber is notified of a new topic publisher, it contacts -## the publisher to establish a connection. The subscriber gathers a -## list of supported protocols (e.g. [['TCPROS'], ['MEMMAP']]) from -## its protocol handlers (L{get_supported}) and then passes these to -## the publisher. Each of these protocols is actual a list, -## e.g. ['MPI', LaneWidth, BusSpeed], since a protocol may have -## associated parameters. This is considered the start of the -## 'negotiation phase'. -## -## subscriber -> pub.requestTopic(protocols) -## -## The Publisher selects a protocol from the lists and tells the -## appropriate protocol handler to prepare the outbound connection: -## -## pub.requestTopic() -> pub.protocol_handler.init_publisher(selected_protocol) -## -## The protocol handler will return a new set of parameters -## representing connection parameters, e.g. [TCPROS, address, -## port]. These new parameters are passed back to the subscriber, -## which tells its protocol handler to establish the connection. -## -## subscriber -> subscriber.protocol_handler.create_transport(protocolParams) -class ProtocolHandler(object): #interface - - ## shutdown any resources associated with handling this protocol - ## @param self - def shutdown(self): - pass - - ## Create a new Transport using the specified \a protocol_params - ## returned from the Publisher \a pub_uri. - ## @param self - ## @param protocol_params [[str, val*]]: parameter list from Publisher. Actual - ## contents are protocol-specified. - ## @param pub_uri str: publisher API URI - ## @param topic str: topic name - ## @return int, str, int: code, message, debug - def create_transport(self, topic, pub_uri, protocol_params): - raise Exception("interface impl") - - ## @param self - ## @param protocol str: protocol name. Must match string identifier used in - ## negotiation. - ## @return bool: True if this handler supports the specified protocol""" - def supports(self, protocol): - return False - - ## This method is called on subscribers and returns the protocol list - ## @param self - ## @return [[str, val*]]: list of supported protocol params. Each set of protocol params is a - ## list where the first element is the string identifier for the protocol. - def get_supported(self): - return [] - - ## Prepare a transport based on one of the supported protocols - ## declared by a Subscriber. Subscribers supply a list of - ## supported protocols, of which one is selected by the Publisher - ## and passed to init_publisher(). init_publisher is responsible - ## for initializing the publisher based on the selection. - ## @param self - ## @param topic str: name of topic - ## @param protocol: selected protocol parameters from the Subscriber. - ## @return (int, str, list): (code, statusMessage, params). params - ## is protocol specific. These params will be sent to the Subscriber - ## so that it can create_transport(). - def init_publisher(self, topic, protocol): - raise Exception("interface impl") - diff --git a/core/rospy/src/rospy/impl/udpros.py b/core/rospy/src/rospy/impl/udpros.py deleted file mode 100644 index 445f8e38..00000000 --- a/core/rospy/src/rospy/impl/udpros.py +++ /dev/null @@ -1,300 +0,0 @@ -# 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$ -""" -UDPROS connection protocol. -""" -## UDPROS connection protocol. -# http://ros.org/wiki/ROS/UDPROS -# - -import roslib.network - -import rospy.impl.registration -import rospy.impl.transport - -def get_max_datagram_size(): - #TODO - return 1024 - -class UDPROSHandler(rospy.transport.ProtocolHandler): - """ - rospy protocol handler for UDPROS. Stores the datagram server if necessary. - """ - - def __init__(self, port=0): - """ - ctor - """ - self.port = port - self.buff_size = get_max_datagram_size() - - def init_server(self): - """ - Initialize and start the server thread, if not already initialized. - """ - if self.server is not None: - return - s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) - s.bind((roslib.network.get_bind_address(), self.port)) - if self.port == 0: - self.port = s.getsockname()[1] - self.server = s - threading.start_new_thread(self.run, ()) - - def run(self): - buff_size = self.buff_size - try: - while not rospy.core.is_shutdown(): - data = self.server.recvfrom(self.buff_size) - print "received packet" - #TODO - except: - #TODO: log - pass - - def shutdown(self): - if self.sock is not None: - self.sock.close() - - def create_transport(self, topic_name, pub_uri, protocol_params): - """ - Connect to topic resolved_name on Publisher pub_uri using UDPROS. - @param resolved_name str: resolved topic name - @type resolved_name: str - @param pub_uri: XML-RPC URI of publisher - @type pub_uri: str - @param protocol_params: protocol parameters to use for connecting - @type protocol_params: [XmlRpcLegal] - @return: code, message, debug - @rtype: (int, str, int) - """ - - #Validate protocol params = [UDPROS, address, port, headers] - if type(protocol_params) != list or len(protocol_params) != 4: - return 0, "ERROR: invalid UDPROS parameters", 0 - if protocol_params[0] != UDPROS: - return 0, "INTERNAL ERROR: protocol id is not UDPROS: %s"%id, 0 - - #TODO: get connection_id and buffer size from params - id, dest_addr, dest_port, headers = protocol_params - - self.init_server() - - #TODO: parse/validate headers - - sub = rospy.registration.get_topic_manager().get_subscriber_impl(topic_name) - # Create Transport - - # TODO: create just a single 'connection' instance to represent - # all UDP connections. 'connection' can take care of unifying - # publication if addresses are the same - transport = UDPTransport(protocol, topic_name, sub.receive_callback) - - # Attach connection to _SubscriberImpl - if sub.add_connection(transport): #pass udp connection to handler - return 1, "Connected topic[%s]. Transport impl[%s]"%(topic_name, transport.__class__.__name__), dest_port - else: - transport.close() - return 0, "ERROR: Race condition failure: duplicate topic subscriber [%s] was created"%(topic_name), 0 - - def supports(self, protocol): - """ - @param protocol: name of protocol - @type protocol: str - @return: True if protocol is supported - @rtype: bool - """ - return protocol == UDPROS - - def get_supported(self): - """ - Get supported protocols - """ - return [[UDPROS]] - - def init_publisher(self, topic_name, protocol_params): - """ - Initialize this node to start publishing to a new UDP location. - - @param resolved_name: topic name - @type resolved__name: str - - @param protocol_params: requested protocol - parameters. protocol[0] must be the string 'UDPROS' - @type protocol_params: [str, value*] - @return: (code, msg, [UDPROS, addr, port]) - @rtype: (int, str, list) - """ - - if protocol_params[0] != UDPROS: - return 0, "Internal error: protocol does not match UDPROS: %s"%protocol, [] - #TODO - _, header, host, port, max_datagram_size = protocol_params - #TODO: connection_id, max_datagraph_size - return 1, "ready", [UDPROS] - - def topic_connection_handler(self, sock, client_addr, header): - """ - Process incoming topic connection. Reads in topic name from - handshake and creates the appropriate L{TCPROSPub} handler for the - connection. - @param sock: socket connection - @type sock: socket.socket - @param client_addr: client address - @type client_addr: (str, int) - @param header: key/value pairs from handshake header - @type header: dict - @return: error string or None - @rtype: str - """ - for required in ['topic', 'md5sum', 'callerid']: - if not required in header: - return "Missing required '%s' field"%required - else: - resolved_topic_name = header['topic'] - md5sum = header['md5sum'] - tm = rospy.registration.get_topic_manager() - topic = tm.get_publisher_impl(resolved_topic_name) - if not topic: - return "[%s] is not a publisher of [%s]. Topics are %s"%(rospy.names.get_caller_id(), resolved_topic_name, tm.get_publications()) - elif md5sum != rospy.names.TOPIC_ANYTYPE and md5sum != topic.data_class._md5sum: - - actual_type = topic.data_class._type - - # check to see if subscriber sent 'type' header. If they did, check that - # types are same first as this provides a better debugging message - if 'type' in header: - requested_type = header['type'] - if requested_type != actual_type: - return "topic types do not match: [%s] vs. [%s]"%(requested_type, actual_type) - else: - # defaults to actual type - requested_type = actual_type - - return "Client [%s] wants topic [%s] to have datatype/md5sum [%s/%s], but our version has [%s/%s] Dropping connection."%(header['callerid'], resolved_topic_name, requested_type, md5sum, actual_type, topic.data_class._md5sum) - - else: - #TODO:POLLING if polling header is present, have to spin up receive loop as well - - # #1334: tcp_nodelay support from subscriber option - if 'tcp_nodelay' in header: - tcp_nodelay = True if header['tcp_nodelay'].strip() == '1' else False - else: - tcp_nodelay = self.tcp_nodelay_map.get(resolved_topic_name, False) - - _configure_pub_socket(sock, tcp_nodelay) - protocol = TCPROSPub(resolved_topic_name, topic.data_class, is_latch=topic.is_latch, headers=topic.headers) - transport = TCPROSTransport(protocol, resolved_topic_name) - transport.set_socket(sock, header['callerid']) - transport.write_header() - topic.add_connection(transport) - - - -## UDPROS communication routines -class UDPROSTransport(rospy.transport.Transport): - transport_type = 'UDPROS' - - def __init__(self, protocol, name, header): - """ - ctor - @param name: topic name - @type name: str: - @param protocol: protocol implementation - @param protocol: UDPROSTransportProtocol - @param header: handshake header if transport handshake header was - already read off of transport. - @type header: dict - @throws TransportInitError: if transport cannot be initialized according to arguments - """ - super(UDPROSTransport, self).__init__(protocol.direction, name=name) - if not name: - raise TransportInitError("Unable to initialize transport: name is not set") - - self.done = False - self.header = header - - def send_message(self, msg, seq): - """ - Convenience routine for services to send a message across a - particular connection. NOTE: write_data is much more efficient - if same message is being sent to multiple connections. Not - threadsafe. - @param msg: message to send - @type msg: Msg - @param seq: sequence number for message - @type seq: int - @raise TransportException: if error occurred sending message - """ - # this will call write_data(), so no need to keep track of stats - serialize_message(self.write_buff, seq, msg) - self.write_data(self.write_buff.getvalue()) - self.write_buff.truncate(0) - - def write_data(self, data): - """ - Write raw data to transport - @raise TransportInitialiationError: could not be initialized - @raise TransportTerminated: no longer open for publishing - """ - # TODO - # - cut into packets - # write to address - pass - - def receive_once(self): - """ - block until messages are read off of socket - @return: list of newly received messages - @rtype: [Msg] - @raise TransportException: if unable to receive message due to error - """ - pass - - ## Receive messages until shutdown - ## @param self - ## @param msgs_callback fn([msg]): callback to invoke for new messages received - def receive_loop(self, msgs_callback): - pass - - ## close i/o and release resources - def close(super): - self(UDPROSTransport, self).close() - #TODO - self.done = True - -_handler = UDPROSHandler() - -def get_handler(): - return _handler diff --git a/core/rospy/src/rospy/impl/validators.py b/core/rospy/src/rospy/impl/validators.py deleted file mode 100644 index b2011a80..00000000 --- a/core/rospy/src/rospy/impl/validators.py +++ /dev/null @@ -1,51 +0,0 @@ -# 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$ - -"""Internal-use Python decorators for parameter validation""" - -class ParameterInvalid(Exception): - """Exception that is raised when a parameter fails validation checks""" - def __init__(self, message): - self._message = message - - def __str__(self): - return str(self._message) - -def non_empty(param_name): - """Validator that checks that parameter is not empty""" - def validator(param, context): - if not param: - raise ParameterInvalid("ERROR: parameter [%s] must be specified and non-empty"%param_name) - return param - return validator diff --git a/core/rospy/src/rospy/msg.py b/core/rospy/src/rospy/msg.py deleted file mode 100644 index 7842a2f5..00000000 --- a/core/rospy/src/rospy/msg.py +++ /dev/null @@ -1,245 +0,0 @@ -# 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$ - -"""Internal use: Support for ROS messages, including network serialization routines""" - -import types -import time -import struct -import logging -import traceback - -import roslib.message -import rospy.core -import rospy.names - -class AnyMsg(roslib.message.Message): - """ - Message class to use for subscribing to any topic regardless - of type. Incoming messages are not deserialized. Instead, the raw - serialized data can be accssed via the buff property. - - This class is meant to be used by advanced users only. - """ - _md5sum = rospy.names.TOPIC_ANYTYPE - _type = rospy.names.TOPIC_ANYTYPE - _has_header = False - _full_text = '' - __slots__ = ['_buff'] - def __init__(self, *args): - """ - Constructor. Does not accept any arguments. - """ - if len(args) != 0: - raise rospy.exceptions.ROSException("AnyMsg does not accept arguments") - self._buff = None - - def serialize(self, buff): - """AnyMsg provides an implementation so that a node can forward messages w/o (de)serialization""" - if self._buff is None: - raise rospy.exceptions("AnyMsg is not initialized") - else: - buff.write(self._buff) - - def deserialize(self, str): - """Copies raw buffer into self._buff""" - self._buff = str - return self - -def args_kwds_to_message(data_class, args, kwds): - """ - Given a data class, take in the args and kwds of a function call and return the appropriate - data_class instance. - - If kwds are specified, a new data_class instance will be created with keyword-style init. - - If there is only a single arg and it is of the correct type, it - will be returned. AnyMsg is considered to match all data_class - types. - - Otherwise, args will be used as args for a new message instance. - - @param data_class: Message class that will be used to instantiate new instances, if necessary. - @type data_class: Message class - @param args: function args - @type args: sequence - @param kwds: function kwds - @type kwds: dict - @raise TypeError: if args and kwds are both specified - """ - if args and kwds: - raise TypeError("publish() can be called with arguments or keywords, but not both.") - elif kwds: - return data_class(**kwds) - else: - if len(args) == 1: - arg = args[0] - # #2584: have to compare on md5sum as isinstance check can fail in dyngen case - if hasattr(arg, '_md5sum') and (arg._md5sum == data_class._md5sum or isinstance(arg, AnyMsg)): - return arg - # If the argument is a message, make sure that it matches - # the type of the first field. This means that the - # data_class has fields and that the type matches. This - # branch isn't necessary but provides more useful - # information to users - elif isinstance(arg, roslib.message.Message) and \ - (len(data_class._slot_types) == 0 or \ - arg._type != data_class._slot_types[0]): - raise TypeError("expected [%s] but got [%s]"%(data_class._slot_types[0], arg._type)) - else: - return data_class(*args) - else: - return data_class(*args) - -def serialize_message(b, seq, msg): - """ - Serialize the message to the buffer - @param b: buffer to write to. WARNING: buffer will be reset after call - @type b: StringIO - @param msg: message to write - @type msg: Message - @param seq: current sequence number (for headers) - @type seq: int: current sequence number (for headers) - @raise ROSSerializationException: if unable to serialize - message. This is usually due to a type error with one of the - fields. - """ - start = b.tell() - b.seek(start+4) #reserve 4-bytes for length - - #update Header object in top-level message - if getattr(msg.__class__, "_has_header", False): - header = msg.header - header.seq = seq - # default value for frame_id is '0' - if header.frame_id is None: - header.frame_id = "0" - - #serialize the message data - try: - msg.serialize(b) - except struct.error, e: - raise rospy.exceptions.ROSSerializationException(e) - - #write 4-byte packet length - # -4 don't include size of length header - end = b.tell() - size = end - 4 - start - b.seek(start) - b.write(struct.pack('= 4) or (size > -1 and left >= size): - # - read in the packet length - # NOTE: size is not inclusive of itself. - if size < 0 and left >= 4: - (size,) = struct.unpack(' -1 and left >= size: - buffs.append(b.read(size)) - pos += size + 4 - left = btell - pos - size = -1 - if max_msgs and len(buffs) >= max_msgs: - break - - #Before we deserialize, prune our buffers baed on the - #queue_size rules. - if queue_size is not None: - buffs = buffs[-queue_size:] - - # Deserialize the messages into msg_queue, then trim the - # msg_queue as specified. - for q in buffs: - data = data_class() - msg_queue.append(data.deserialize(q)) - if queue_size is not None: - del msg_queue[:-queue_size] - - #update buffer b to its correct write position. - if btell == pos: - #common case: no leftover data, reset the buffer - b.seek(start) - b.truncate(start) - else: - if pos != start: - #next packet is stuck in our buffer, copy it to the - #beginning of our buffer to keep things simple - b.seek(pos) - leftovers = b.read(btell-pos) - b.truncate(start + len(leftovers)) - b.seek(start) - b.write(leftovers) - else: - b.seek(btell) - except Exception, e: - logging.getLogger('rospy.msg').error("cannot deserialize message: EXCEPTION %s", traceback.format_exc()) - raise roslib.message.DeserializationError("cannot deserialize: %s"%str(e)) - diff --git a/core/rospy/src/rospy/msproxy.py b/core/rospy/src/rospy/msproxy.py deleted file mode 100644 index 80fd4797..00000000 --- a/core/rospy/src/rospy/msproxy.py +++ /dev/null @@ -1,197 +0,0 @@ -# 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$ -""" -Master/Slave XML-RPC Wrappers. - -The L{MasterProxy} simplifies usage of master/slave -APIs by automatically inserting the caller ID and also adding python -dictionary accessors on the parameter server. -""" - -import rospy.core -import rospy.exceptions -import rospy.names - -import rospy.impl.paramserver -import rospy.impl.masterslave - -_master_arg_remap = { - 'deleteParam': [0], # remap key - 'setParam': [0], # remap key - 'getParam': [0], # remap key - 'searchParam': [0], # remap key - 'subscribeParam': [0], # remap key - 'unsubscribeParam': [0], # remap key - 'hasParam': [0], # remap key - 'registerService': [0], # remap service - 'lookupService': [0], # remap service - 'unregisterService': [0], # remap service - 'registerSubscriber': [0], # remap topic - 'unregisterSubscriber': [0], # remap topic - 'registerPublisher': [0], # remap topic - 'unregisterPublisher': [0], # remap topic - 'lookupNode': [0], # remap node - 'getPublishedTopics': [0], # remap subgraph - } - -class MasterProxy(object): - """ - Convenience wrapper for ROS master API and XML-RPC - implementation. The Master API methods can be invoked on this - object and will be forwarded appropriately. Names in arguments - will be remapped according to current node settings. Provides - dictionary-like access to parameter server, e.g.:: - - master[key] = value - - """ - - def __init__(self, uri): - """ - Constructor for wrapping a remote master instance. - @param uri: XML-RPC URI of master - @type uri: str - """ - self.target = rospy.core.xmlrpcapi(uri) - - def __getattr__(self, key): #forward api calls to target - f = getattr(self.target, key) - if key in _master_arg_remap: - remappings = _master_arg_remap[key] - else: - remappings = rospy.impl.masterslave.ROSHandler.remappings(key) - def wrappedF(*args, **kwds): - args = [rospy.names.get_caller_id(),]+list(args) - #print "Remap indicies", remappings - for i in remappings: - i = i + 1 #callerId does not count - #print "Remap %s => %s"%(args[i], rospy.names.resolve_name(args[i])) - args[i] = rospy.names.resolve_name(args[i]) - return f(*args, **kwds) - return wrappedF - - def __getitem__(self, key): - """ - Fetch item from parameter server and subscribe to future updates so that - values can be cached. - @param key: parameter key - @type key: str - @raise KeyError: if key is not set - """ - #NOTE: remapping occurs here! - resolved_key = rospy.names.resolve_name(key) - if 1: # disable param cache - code, msg, value = self.target.getParam(rospy.names.get_caller_id(), resolved_key) - if code != 1: #unwrap value with Python semantics - raise KeyError(key) - return value - - try: - # check for value in the parameter server cache - return rospy.impl.paramserver.get_param_server_cache().get(resolved_key) - except KeyError: - # first access, make call to parameter server - code, msg, value = self.target.subscribeParam(rospy.names.get_caller_id(), rospy.core.get_node_uri(), resolved_key) - if code != 1: #unwrap value with Python semantics - raise KeyError(key) - # set the value in the cache so that it's marked as subscribed - rospy.impl.paramserver.get_param_server_cache().set(resolved_key, value) - return value - - def __setitem__(self, key, val): - """ - Set parameter value on Parameter Server - @param key: parameter key - @type key: str - @param val: parameter value - @type val: XMLRPC legal value - """ - self.target.setParam(rospy.names.get_caller_id(), rospy.names.resolve_name(key), val) - - def search_param(self, key): - """ - Search for a parameter matching key on the parameter server - @return: found key or None if search did not succeed - @rtype: str - @raise ROSException: if parameter server reports an error - """ - # #1810 searchParam has to use unresolved form of mappings - mappings = rospy.names.get_mappings() - if key in mappings: - key = mappings[key] - code, msg, val = self.target.searchParam(rospy.names.get_caller_id(), key) - if code == 1: - return val - elif code == -1: - return None - else: - raise rospy.exceptions.ROSException("cannot search for parameter: %s"%msg) - - def __delitem__(self, key): - """ - Delete parameter key from the parameter server. - @raise KeyError: if key is not set - @raise ROSException: if parameter server reports an error - """ - resolved_key = rospy.names.resolve_name(key) - code, msg, _ = self.target.deleteParam(rospy.names.get_caller_id(), resolved_key) - if code == -1: - raise KeyError(key) - elif code != 1: - raise rospy.exceptions.ROSException("cannot delete parameter: %s"%msg) - elif 0: #disable parameter cache - # set the value in the cache so that it's marked as subscribed - rospy.impl.paramserver.get_param_server_cache().delete(resolved_key) - - def __contains__(self, key): - """ - Check if parameter is set on Parameter Server - @param key: parameter key - @type key: str - @raise ROSException: if parameter server reports an error - """ - code, msg, value = self.target.hasParam(rospy.names.get_caller_id(), rospy.names.resolve_name(key)) - if code != 1: - raise rospy.exceptions.ROSException("cannot check parameter on server: %s"%msg) - return value - - def __iter__(self): - """ - @raise ROSException: if parameter server reports an error - """ - code, msg, value = self.target.getParamNames(rospy.names.get_caller_id()) - if code == 1: - return value.__iter__() - else: - raise rospy.exceptions.ROSException("cannot retrieve parameter names: %s"%msg) diff --git a/core/rospy/src/rospy/names.py b/core/rospy/src/rospy/names.py deleted file mode 100644 index 756ce3d7..00000000 --- a/core/rospy/src/rospy/names.py +++ /dev/null @@ -1,328 +0,0 @@ -# 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$ - -""" -Support for ROS Names - -See: U{http://www.ros.org/wiki/Names} -""" - -import sys -import os -from itertools import ifilter - -from roslib.names import namespace, get_ros_namespace, ns_join, make_global_ns, load_mappings, \ - SEP, GLOBALNS, TYPE_SEPARATOR, REMAP, ANYTYPE, \ - is_global, is_private -import roslib.names - -from rospy.exceptions import ROSException -from rospy.impl.validators import ParameterInvalid - -TOPIC_ANYTYPE = ANYTYPE #indicates that a subscriber will connect any datatype given to it -SERVICE_ANYTYPE = ANYTYPE #indicates that a service client does not have a fixed type - -def canonicalize_name(name): - """ - Put name in canonical form. Double slashes '//' are removed and - name is returned without any trailing slash, e.g. /foo/bar - @param name: ROS name - @type name: str - """ - if not name or name == SEP: - return name - elif name[0] == SEP: - return '/' + '/'.join([x for x in name.split(SEP) if x]) - else: - return '/'.join([x for x in name.split(SEP) if x]) - ##if len(name) > 1 and name[-1] == SEP: - ## return name[:-1] - ##return name - -# Mappings override name resolution by substituting fully-qualified -# names in for local name references. They override any name -# reference, with exception of '.local' names. We load remapping args -# as soon as client API is referenced so that they are initialized -# before Topic constructors are invoked. -_mappings = load_mappings(sys.argv) -_resolved_mappings = {} - -def reload_mappings(argv): - """ - Re-initialize the name remapping table. - - @param argv: Command line arguments to this program. ROS reads - these arguments to find renaming params. - @type argv: [str] - """ - global _mappings - _mappings = load_mappings(argv) - -# #1810 -def initialize_mappings(node_name): - """ - Initialize the remapping table based on provide node name. - - @param node_name: name of node (caller ID) - @type node_name: str - """ - global _resolved_mappings - _resolved_mappings = {} - for m,v in _mappings.iteritems(): - # resolve both parts of the mappings. use the roslib.names - # version of resolve_name to avoid circular mapping. - if m.startswith('__'): # __name, __log, etc... - _resolved_mappings[m] = v - else: - _resolved_mappings[roslib.names.resolve_name(m, node_name)] = roslib.names.resolve_name(v, node_name) - -def resolve_name_without_node_name(name): - """ - The need for this function is complicated -- Topics and Services can be created before init_node is called. - In general, this is okay, unless the name is a ~name, in which - case we have to raise an ValueError - - @param name: ROS name to resolve - @type name: str - @raise ValueError: if name is a ~name - @raise ROSInitException: if name is remapped to a ~name - """ - if is_private(name): - raise ValueError("~name topics cannot be created before init_node() has been called") - - # we use the underlying roslib.names.resolve_name to avoid dependencies on nodename/remappings - fake_caller_id = ns_join(get_namespace(), 'node') - fake_resolved = roslib.names.resolve_name(name, fake_caller_id) - - for m, v in _mappings.iteritems(): - if roslib.names.resolve_name(m, fake_caller_id) == fake_resolved: - if is_private(name): - raise ROSInitException("due to the way this node is written, %s cannot be remapped to a ~name. \nThe declaration of topics/services must be moved after the call to init_node()"%name) - else: - return roslib.names.resolve_name(v, fake_caller_id) - return fake_resolved - -def get_mappings(): - """ - Get mapping table with unresolved names - - @return: command-line remappings {name: name} - @rtype: {str: str} - """ - return _mappings - -def get_resolved_mappings(): - """ - Get mapping table with resolved names - - @return: command-line remappings {name: name} - @rtype: {str: str} - """ - return _resolved_mappings - -#TODO: port to a wrapped call to roslib.names.resolve_name -def resolve_name(name, caller_id=None): - """ - Resolve a ROS name to its global, canonical form. Private ~names - are resolved relative to the node name. - - @param name: name to resolve. - @type name: str - @param caller_id: node name to resolve relative to. To - resolve to local namespace, omit this parameter (or use None) - @type caller_id: str - @return: Resolved name. If name is empty/None, resolve_name - returns parent namespace. If namespace is empty/None, - @rtype: str - """ - if not caller_id: - caller_id = get_name() - if not name: #empty string resolves to namespace - return namespace(caller_id) - - name = canonicalize_name(name) - if name[0] == SEP: #global name - resolved_name = name - elif is_private(name): #~name - resolved_name = ns_join(caller_id, name[1:]) - else: #relative - resolved_name = namespace(caller_id) + name - - #Mappings override general namespace-based resolution - # - do this before canonicalization as remappings are meant to - # match the name as specified in the code - if resolved_name in _resolved_mappings: - return _resolved_mappings[resolved_name] - else: - return resolved_name - - -def remap_name(name, caller_id=None, resolved=True): - """ - Remap a ROS name. This API should be used to instead of - resolve_name for APIs in which you don't wish to resolve the name - unless it is remapped. - @param name: name to remap - @type name: str - - @param resolved: if True (default), use resolved names in remappings, which is the standard for ROS. - @type resolved: bool - - @return: Remapped name - @rtype: str - """ - if not caller_id: - caller_id = get_caller_id() - if name in _mappings: - return roslib.names.resolve_name(_mappings[name], caller_id) - return name - -def scoped_name(caller_id, name): - """ - Convert the global caller_id to a relative name within the namespace. For example, for - namespace '/foo' and name '/foo/bar/name', the return value will - be 'bar/name' - - WARNING: scoped_name does not validate that name is actually within - the supplied namespace. - @param caller_id: caller ID, in canonical form - @type caller_id: str - @param name: name to scope - @type name: str - @return: name scoped to the caller_id's namespace. - @rtype: str - """ - if not is_global(caller_id): - raise ROSException("caller_id must be global") - return canonicalize_name(name)[len(namespace(caller_id)):] - - -################################################### -# Name validators ############################ - -#Technically XMLRPC will never send a None, but I don't want to code masterslave.py to be -#XML-RPC specific in this way. - -def valid_name_validator_resolved(param_name, param_value, caller_id): - if not param_value or not isinstance(param_value, basestring): - raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) - #TODO: actual validation of chars - # I added the colon check as the common error will be to send an URI instead of name - if ':' in param_value or ' ' in param_value: - raise ParameterInvalid("ERROR: parameter [%s] contains illegal chars"%param_name) - #don't use our own resolve_name because we do not want to remap - return roslib.names.resolve_name(param_value, caller_id, remappings=None) -def valid_name_validator_unresolved(param_name, param_value, caller_id): - if not param_value or not isinstance(param_value, basestring): - raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) - #TODO: actual validation of chars - # I added the colon check as the common error will be to send an URI instead of name - if ':' in param_value or ' ' in param_value: - raise ParameterInvalid("ERROR: parameter [%s] contains illegal chars"%param_name) - return param_value - -def valid_name(param_name, resolve=True): - """ - Validator that resolves names and also ensures that they are not empty - @param param_name: name - @type param_name: str - @param resolve: if True/omitted, the name will be resolved to - a global form. Otherwise, no resolution occurs. - @type resolve: bool - @return: resolved parameter value - @rtype: str - """ - def validator(param_value, caller_id): - if resolve: - return valid_name_validator_resolved(param_name, param_value, caller_id) - return valid_name_validator_unresolved(param_name, param_value, caller_id) - return validator - -def global_name(param_name): - """ - Validator that checks for valid, global graph resource name. - @return: parameter value - @rtype: str - """ - def validator(param_value, caller_id): - if not param_value or not isinstance(param_value, basestring): - raise ParameterInvalid("ERROR: parameter [%s] must be a non-empty string"%param_name) - #TODO: actual validation of chars - if not is_global(param_value): - raise ParameterInvalid("ERROR: parameter [%s] must be a globally referenced name"%param_name) - return param_value - return validator - -######################################################### -#Global Namespace Routines -# - Global state, e.g. singletons and namespace - -_caller_namespace = get_ros_namespace() -_caller_id = _caller_namespace+'unnamed' #default for non-node. - -def get_namespace(): - """ - Get namespace of local node. - @return: fully-qualified name of local node or '' if not applicable - @rtype: str - """ - return _caller_namespace - -def get_name(): - """ - Get fully resolved name of local node. If this is not a node, - use empty string - @return: fully-qualified name of local node or '' if not applicable - @rtype: str - """ - return _caller_id - -# backwards compatibility -get_caller_id = get_name - -def _set_caller_id(caller_id): - """ - Internal API. - Set the global name (i.e. caller_id) and namespace. Methods can - check what the name of the current node is by calling get_caller_id. - - The caller_id is important as it is the first parameter to any API - call on a remote node. Invoked by ROSNode constructor - @param caller_id: new caller ID - @type caller_id: str - """ - global _caller_id, _caller_namespace - _caller_id = caller_id - _caller_namespace = namespace(caller_id) - diff --git a/core/rospy/src/rospy/numpy_msg.py b/core/rospy/src/rospy/numpy_msg.py deleted file mode 100644 index 92fa8998..00000000 --- a/core/rospy/src/rospy/numpy_msg.py +++ /dev/null @@ -1,93 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, 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$ - -""" -Support for using numpy with rospy messages. - -For tutorials, see U{http://www.ros.org/wiki/rospy_tutorials/Tutorials/numpy} - -Listener example:: - from rospy.numpy_msg import numpy_msg - - rospy.init_node('mynode') - rospy.Subscriber("mytopic", numpy_msg(TopicType) - -Publisher example:: - - from rospy.numpy_msg import numpy_msg - import numpy - - pub = rospy.Publisher('mytopic', numpy_msg(TopicType)) - rospy.init_node('mynode') - a = numpy.array([1.0, 2.1, 3.2, 4.3, 5.4, 6.5], dtype=numpy.float32) - pub.publish(a) -""" - -import numpy - -# TODO: we will need to generate a new type structure with -# little-endian specified and then pass that type structure into the -# *_numpy calls. - -def _serialize_numpy(self, buff): - """ - wrapper for factory-generated class that passes numpy module into serialize - """ - # pass in numpy module reference to prevent import in auto-generated code - return self.serialize_numpy(buff, numpy) - -def _deserialize_numpy(self, str): - """ - wrapper for factory-generated class that passes numpy module into deserialize - """ - # pass in numpy module reference to prevent import in auto-generated code - return self.deserialize_numpy(str, numpy) - -## Use this function to generate message instances using numpy array -## types for numerical arrays. -## @msg_type Message class: call this functioning on the message type that you pass -## into a Publisher or Subscriber call. -## @returns Message class -def numpy_msg(msg_type): - classdict = { '__slots__': msg_type.__slots__, '_slot_types': msg_type._slot_types, - '_md5sum': msg_type._md5sum, '_type': msg_type._type, - '_has_header': msg_type._has_header, '_full_text': msg_type._full_text, - 'serialize': _serialize_numpy, 'deserialize': _deserialize_numpy, - 'serialize_numpy': msg_type.serialize_numpy, - 'deserialize_numpy': msg_type.deserialize_numpy - } - - # create the numpy message type - msg_type_name = "Numpy_%s"%msg_type._type.replace('/', '__') - return type(msg_type_name,(msg_type,),classdict) diff --git a/core/rospy/src/rospy/rostime.py b/core/rospy/src/rospy/rostime.py deleted file mode 100644 index fae28935..00000000 --- a/core/rospy/src/rospy/rostime.py +++ /dev/null @@ -1,272 +0,0 @@ -# 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$ - -""" -ROS time and duration representations, as well as internal routines -for managing wallclock versus a simulated clock. The important data -classes are L{Time} and L{Duration}, which represent the ROS 'time' -and 'duration' primitives, respectively. -""" - -import threading -import time -import traceback - -import rospy.exceptions - -import roslib.msg -import roslib.rosenv -import roslib.rostime - -## /time support. This hooks into the rospy Time representation and -## allows it to be overriden with data from the /time topic. - -_rostime_initialized = False -_rostime_current = None -_rostime_cond = threading.Condition() - -# subclass roslib to provide abstraction layer -class Duration(roslib.rostime.Duration): - """ - Duration represents the ROS 'duration' primitive type, which - consists of two integers: seconds and nanoseconds. The Duration - class allows you to add and subtract Duration instances, including - adding and subtracting from L{Time} instances. - - Usage:: - five_seconds = Duration(5) - five_nanoseconds = Duration(0, 5) - - print 'Fields are', five_seconds.secs, five_seconds.nsecs - - # Duration arithmetic - ten_seconds = five_seconds + five_seconds - five_secs_ago = rospy.Time.now() - five_seconds # Time minus Duration is a Time - - true_val = ten_second > five_seconds - """ - __slots__ = [] - - def __init__(self, secs=0, nsecs=0): - """ - Create new Duration instance. secs and nsecs are integers and - correspond to the ROS 'duration' primitive type. - - @param secs: seconds - @type secs: int - @param nsecs: nanoseconds - @type nsecs: int - """ - roslib.rostime.Duration.__init__(self, secs, nsecs) - -class Time(roslib.rostime.Time): - """ - Time represents the ROS 'time' primitive type, which consists of two - integers: seconds since epoch and nanoseconds since seconds. Time - instances are mutable. - - The L{Time.now()} factory method can initialize Time to the - current ROS time and L{from_sec()} can be used to create a - Time instance from the Python's time.time() float seconds - representation. - - The Time class allows you to subtract Time instances to compute - Durations, as well as add Durations to Time to create new Time - instances. - - Usage:: - now = rospy.Time.now() - zero_time = rospy.Time() - - print 'Fields are', now.secs, now.nsecs - - # Time arithmetic - five_secs_ago = now - rospy.Duration(5) # Time minus Duration is a Time - five_seconds = now - five_secs_ago # Time minus Time is a Duration - true_val = now > five_secs_ago - - # NOTE: in general, you will want to avoid using time.time() in ROS code - import time - py_time = rospy.Time.from_sec(time.time()) - """ - __slots__ = [] - - def __init__(self, secs=0, nsecs=0): - """ - Constructor: secs and nsecs are integers and correspond to the - ROS 'time' primitive type. You may prefer to use the static - L{from_sec()} and L{now()} factory methods instead. - - @param secs: seconds since epoch - @type secs: int - @param nsecs: nanoseconds since seconds (since epoch) - @type nsecs: int - """ - roslib.rostime.Time.__init__(self, secs, nsecs) - - def now(): - """ - Create new L{Time} instance representing current time. This - can either be wall-clock time or a simulated clock. It is - strongly recommended that you use the now() factory to create - current time representations instead of reading wall-clock - time and create Time instances from it. - - @return: L{Time} instance for current time - @rtype: L{Time} - """ - if not _rostime_initialized: - raise rospy.exceptions.ROSInitException("time is not initialized. Have you called init_node()?") - if _rostime_current is not None: - # initialize with sim time - return _rostime_current - else: - # initialize with wallclock - float_secs = time.time() - secs = int(float_secs) - nsecs = int((float_secs - secs) * 1000000000) - return Time(secs, nsecs) - - now = staticmethod(now) - - # have to reproduce super class implementation to return correct typing - - def from_seconds(float_secs): - """ - Use Time.from_sec() instead. Retained for backwards compatibility. - - @param float_secs: time value in time.time() format - @type float_secs: float - @return: Time instance for specified time - @rtype: L{Time} - """ - return Time.from_sec(float_secs) - - from_seconds = staticmethod(from_seconds) - - def from_sec(float_secs): - """ - Create new Time instance from a float seconds representation - (e.g. time.time()). - - @param float_secs: time value in time.time() format - @type float_secs: float - @return: Time instance for specified time - @rtype: L{Time} - """ - secs = int(float_secs) - nsecs = int((float_secs - secs) * 1000000000) - return Time(secs, nsecs) - - from_sec = staticmethod(from_sec) - -def _set_rostime(t): - """Callback to update ROS time from a ROS Topic""" - if isinstance(t, roslib.rostime.Time): - t = Time(t.secs, t.nsecs) - elif not isinstance(t, Time): - raise ValueError("must be Time instance") - global _rostime_current - _rostime_current = t - try: - _rostime_cond.acquire() - _rostime_cond.notifyAll() - finally: - _rostime_cond.release() - -def get_rostime(): - """ - Get the current time as a L{Time} object - @return: current time as a L{rospy.Time} object - @rtype: L{Time} - """ - return Time.now() - -def get_time(): - """ - Get the current time as float secs (time.time() format) - @return: time in secs (time.time() format) - @rtype: float - """ - return Time.now().to_sec() - -def set_rostime_initialized(val): - """ - Internal use. - Mark rostime as initialized. This flag enables other routines to - throw exceptions if rostime is being used before the underlying - system is initialized. - @param val: value for initialization state - @type val: bool - """ - global _rostime_initialized - _rostime_initialized = val - -def is_rostime_initialized(): - """ - Internal use. - @return: True if rostime has been initialized - @rtype: bool - """ - return _rostime_initialized - -def get_rostime_cond(): - """ - internal API for helper routines that need to wait on time updates - @return: rostime conditional var - @rtype: threading.Cond - """ - return _rostime_cond - -def is_wallclock(): - """ - Internal use for ROS-time routines. - @return: True if ROS is currently using wallclock time - @rtype: bool - """ - return _rostime_current == None - -def switch_to_wallclock(): - """ - Internal use. - Switch ROS to wallclock time. This is mainly for testing purposes. - """ - global _rostime_current - _rostime_current = None - try: - _rostime_cond.acquire() - _rostime_cond.notifyAll() - finally: - _rostime_cond.release() - diff --git a/core/rospy/src/rospy/service.py b/core/rospy/src/rospy/service.py deleted file mode 100644 index eb9b1213..00000000 --- a/core/rospy/src/rospy/service.py +++ /dev/null @@ -1,147 +0,0 @@ -# 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$ - -"""Base-classes and management of ROS services. -See L{rospy.tcpros_service} for actual implementation.""" - -from __future__ import with_statement - -import logging -import traceback - -from rospy.core import * - -from rospy.impl.registration import set_service_manager, Registration, get_registration_listeners -from rospy.impl.transport import * - -logger = logging.getLogger('rospy.service') - -## alias of roslib.message.ServiceDefinition -import roslib.message -ServiceDefinition = roslib.message.ServiceDefinition - -class ServiceException(Exception): - """Exception class for service-related errors""" - pass - -class _Service(object): - """Internal-use superclass for storing service information""" - def __init__(self, name, service_class): - self.resolved_name = resolve_name(name) #services remap as well - self.service_class = service_class - self.request_class = service_class._request_class - self.response_class = service_class._response_class - self.uri = None #initialize attr - -class ServiceManager(object): - """Keeps track of currently registered services in the ROS system""" - - def __init__(self, registration_listeners=None): - """ - ctor - @param registration_listeners: override default registration listener. - @type registration_listeners: RegistrationListeners - """ - self.map = {} # {name : Service} - self.lock = threading.RLock() - if registration_listeners is None: - self.registration_listeners = get_registration_listeners() - else: - self.registration_listeners = registration_listeners - - def get_services(self): - """ - @return: List of (service_name, service_uri) for all registered services. - @rtype: [(str, str)] - """ - with self.lock: - ret_val = [] - for name, service in self.map.iteritems(): - ret_val.append((name, service.uri)) - services = self.map.values() - return ret_val - - def unregister_all(self): - """ - Unregister all registered services - """ - self.map.clear() - - def register(self, resolved_service_name, service): - """ - Register service with ServiceManager and ROS master - @param resolved_service_name: name of service (resolved) - @type resolved_service_name: str - @param service: Service to register - @type service: L{_Service} - """ - err = None - with self.lock: - if resolved_service_name in self.map: - err = "service [%s] already registered"%resolved_service_name - else: - self.map[resolved_service_name] = service - - # NOTE: this call can potentially take a long time under lock and thus needs to be reimplmented - self.registration_listeners.notify_added(resolved_service_name, service.uri, Registration.SRV) - - if err: - raise ServiceException(err) - - def unregister(self, resolved_service_name, service): - """ - Unregister service with L{ServiceManager} and ROS Master - @param resolved_service_name: name of service - @type resolved_service_name: str - @param service: service implementation - @type service: L{_Service} - """ - with self.lock: - curr = self.map.get(resolved_service_name, None) - if curr == service: - del self.map[resolved_service_name] - - # NOTE: this call can potentially take a long time under lock - self.registration_listeners.notify_removed(resolved_service_name, service.uri, Registration.SRV) - - def get_service(self, resolved_service_name): - """ - @param resolved_service_name: name of service - @type resolved_service_name: str - @return: service implementation - @rtype: _Service - """ - return self.map.get(resolved_service_name, None) - -set_service_manager(ServiceManager()) diff --git a/core/rospy/src/rospy/topics.py b/core/rospy/src/rospy/topics.py deleted file mode 100644 index 4dd00e3a..00000000 --- a/core/rospy/src/rospy/topics.py +++ /dev/null @@ -1,1141 +0,0 @@ -# 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$ - -""" -rospy implementation of topics. - -Client API -========== - -L{Publisher} and L{Subscriber} are the client API for topics. - -Internal Implementation -======================= - -Internally, L{_TopicImpl} instances (_PublisherImpl/_SubscriberImpl) -are used to manage actual transport connections. The L{_TopicManager} -is responsible for tracking the system-wide state of publications and -subscriptions as well as the L{_TopicImpl} instances. More info is below. - -L{_TopicManager} -================ - -The L{_TopicManager} does the backend topic bookkeeping for the local -node. Use L{get_topic_manager()} to access singleton. Actual topic -implementations are done through the -L{_TopicImpl}/L{_PublisherImpl}/L{_SubscriberImpl} hierarchy. Client -code generates instances of type L{Publisher}/L{Subscriber}, which -enable to client to create multiple publishers/subscribers of that -topic that get controlled access to the underlying share connections. - -Common parent classes for all rospy topics. The rospy topic autogenerators -create classes that are children of these implementations. -""" - -from __future__ import with_statement -import struct, cStringIO, thread, threading, logging, time -from itertools import chain -import traceback - -import roslib.message - -from rospy.core import * -from rospy.exceptions import ROSSerializationException, TransportTerminated -from rospy.msg import serialize_message, args_kwds_to_message - -from rospy.impl.registration import get_topic_manager, set_topic_manager, Registration, get_registration_listeners -from rospy.impl.tcpros import get_tcpros_handler, DEFAULT_BUFF_SIZE -from rospy.impl.transport import DeadTransport - -_logger = logging.getLogger('rospy.topics') - -# wrap roslib implementation and map it to rospy namespace -Message = roslib.message.Message - -####################################################################### -# Base classes for all client-API instantiated pub/sub -# -# There are two trees: Topic and _TopicImpl. Topic is the client API -# for interfacing with topics, while _TopicImpl implements the -# underlying connection details. - -class Topic(object): - """Base class of L{Publisher} and L{Subscriber}""" - - def __init__(self, name, data_class, reg_type): - """ - @param name: graph resource name of topic, e.g. 'laser'. - @type name: str - @param data_class: message class for serialization - @type data_class: L{Message} - @param reg_type Registration.PUB or Registration.SUB - @type reg_type: str - @raise ValueError: if parameters are invalid - """ - - if not name or not isinstance(name, basestring): - raise ValueError("topic name is not a non-empty string") - if isinstance(name, unicode): - raise ValueError("topic name cannot be unicode") - if data_class is None: - raise ValueError("topic parameter 'data_class' is not initialized") - if not type(data_class) == type: - raise ValueError("data_class [%s] is not a class"%data_class) - if not issubclass(data_class, roslib.message.Message): - raise ValueError("data_class [%s] is not a message data class"%data_class.__class__.__name__) - # #2202 - if not roslib.names.is_legal_name(name): - import warnings - warnings.warn("'%s' is not a legal ROS graph resource name. This may cause problems with other ROS tools"%name, stacklevel=2) - - # this is a bit ugly, but necessary due to the fact that we allow - # topics and services to be initialized before the node - if not rospy.core.is_initialized(): - self.resolved_name = rospy.names.resolve_name_without_node_name(name) - else: - # init_node() has been called, so we can do normal resolution - self.resolved_name = resolve_name(name) - - self.name = self.resolved_name # #1810 for backwards compatibility - - self.data_class = data_class - self.type = data_class._type - self.md5sum = data_class._md5sum - self.reg_type = reg_type - self.impl = get_topic_manager().acquire_impl(reg_type, self.resolved_name, data_class) - - def get_num_connections(self): - """ - get the number of connections to other ROS nodes for this topic. For a Publisher, - this corresponds to the number of nodes subscribing. For a Subscriber, the number - of publishers. - @return: number of connections - @rtype: int - """ - return self.impl.get_num_connections() - - def unregister(self): - """ - unpublish/unsubscribe from topic. Topic instance is no longer - valid after this call. Additional calls to unregister() have no effect. - """ - # as we don't guard unregister, have to protect value of - # resolved_name for release_impl call - resolved_name = self.resolved_name - if resolved_name and self.impl: - get_topic_manager().release_impl(self.reg_type, resolved_name) - self.impl = self.resolved_name = self.type = self.md5sum = self.data_class = None - -class _TopicImpl(object): - """ - Base class of internal topic implementations. Each topic has a - singleton _TopicImpl implementation for managing the underlying - connections. - """ - - def __init__(self, name, data_class): - """ - Base constructor - @param name: graph resource name of topic, e.g. 'laser'. - @type name: str - @param data_class: message data class - @type data_class: L{Message} - """ - - # #1810 made resolved/unresolved more explicit so we don't accidentally double-resolve - self.resolved_name = resolve_name(name) #NOTE: remapping occurs here! - self.name = self.resolved_name # for backwards compatibility - - self.data_class = data_class - self.type = data_class._type - self.handler = None - self.seq = 0 - # lock is used for to serialize call order that methods that - # modify self.connections. Because add/removing connections is - # a rare event, we go through the extra hassle of making a - # copy of the connections/dead_connections/callbacks lists - # when modifying, then setting the reference to the new copy. - # With this pattern, other code can use these lists without - # having to acquire c_lock - self.c_lock = threading.RLock() - self.connections = [] - self.closed = False - # number of Topic instances using this - self.ref_count = 0 - #STATS - self.dead_connections = [] #for retaining stats on old conns - - def __del__(self): - # very similar to close(), but have to be more careful in a __del__ what we call - if self.closed: - return - if self.connections is not None: - for c in self.connections: - try: - c.close() - except: - pass - del self.connections[:] - del self.dead_connections[:] - self.c_lock = self.dead_connections = self.connections = self.handler = self.data_class = self.type = None - - def close(self): - """close I/O""" - if self.closed: - return - self.closed = True - if self.c_lock is not None: - with self.c_lock: - for c in self.connections: - try: - if c is not None: - c.close() - except: - # seems more logger.error internal than external logerr - _logger.error(traceback.format_exc()) - del self.connections[:] - self.c_lock = self.connections = self.handler = self.data_class = self.type = None - - # note: currently not deleting self.dead_connections. not - # sure what the right policy here as dead_connections is - # for statistics only. they should probably be moved to - # the topic manager instead. - - def get_num_connections(self): - with self.c_lock: - return len(self.connections) - - def has_connection(self, endpoint_id): - """ - Query whether or not a connection with the associated \a - endpoint has been added to this object. - @param endpoint_id: endpoint ID associated with connection. - @type endpoint_id: str - """ - # save reference to avoid lock - conn = self.connections - for c in conn: - if c.endpoint_id == endpoint_id: - return True - return False - - def has_connections(self): - """ - Check to see if this topic is connected to other publishers/subscribers - @return: True if topic is connected - @rtype: bool - """ - if self.connections: - return True - return False - - def add_connection(self, c): - """ - Add a connection to this topic. - @param c: connection instance - @type c: Transport - @return: True if connection was added - @rtype: bool - """ - with self.c_lock: - # c_lock is to make add_connection thread-safe, but we - # still make a copy of self.connections so that the rest of the - # code can use self.connections in an unlocked manner - new_connections = self.connections[:] - new_connections.append(c) - self.connections = new_connections - - # connections make a callback when closed - c.set_cleanup_callback(self.remove_connection) - - return True - - def remove_connection(self, c): - """ - Remove connection from topic. - @param c: connection instance to remove - @type c: Transport - """ - try: - # c_lock is to make remove_connection thread-safe, but we - # still make a copy of self.connections so that the rest of the - # code can use self.connections in an unlocked manner - self.c_lock.acquire() - new_connections = self.connections[:] - new_dead_connections = self.dead_connections[:] - if c in new_connections: - new_connections.remove(c) - new_dead_connections.append(DeadTransport(c)) - self.connections = new_connections - self.dead_connections = new_dead_connections - finally: - self.c_lock.release() - - def get_stats_info(self): # STATS - """ - Get the stats for this topic - @return: stats for topic in getBusInfo() format:: - ((connection_id, destination_caller_id, direction, transport, topic_name, connected)*) - @rtype: list - """ - # save referenceto avoid locking - connections = self.connections - dead_connections = self.dead_connections - return [(c.id, c.endpoint_id, c.direction, c.transport_type, self.resolved_name, True) for c in connections] + \ - [(c.id, c.endpoint_id, c.direction, c.transport_type, self.resolved_name, False) for c in dead_connections] - - def get_stats(self): # STATS - """Get the stats for this topic (API stub)""" - raise Exception("subclasses must override") - -# Implementation note: Subscriber attaches to a _SubscriberImpl -# singleton for that topic. The underlying impl manages the -# connections for that publication and enables thread-safe access - -class Subscriber(Topic): - """ - Class for registering as a subscriber to a specified topic, where - the messages are of a given type. - """ - def __init__(self, name, data_class, callback=None, callback_args=None, - queue_size=None, buff_size=DEFAULT_BUFF_SIZE, tcp_nodelay=False): - """ - Constructor. - - NOTE: for the queue_size and buff_size - parameters, rospy does not attempt to do intelligent merging - between multiple Subscriber instances for the same topic. As - they share the same underlying transport, multiple Subscribers - to the same topic can conflict with one another if they set - these parameters differently. - - @param name: graph resource name of topic, e.g. 'laser'. - @type name: str - @param data_class: data type class to use for messages, - e.g. std_msgs.msg.String - @type data_class: L{Message} class - @param callback: function to call ( fn(data)) when data is - received. If callback_args is set, the function must accept - the callback_args as a second argument, i.e. fn(data, - callback_args). NOTE: Additional callbacks can be added using - add_callback(). - @type callback: str - @param callback_args: additional arguments to pass to the - callback. This is useful when you wish to reuse the same - callback for multiple subscriptions. - @type callback_args: any - @param queue_size: maximum number of messages to receive at - a time. This will generally be 1 or None (infinite, - default). buff_size should be increased if this parameter - is set as incoming data still needs to sit in the incoming - buffer before being discarded. Setting queue_size - buff_size to a non-default value affects all subscribers to - this topic in this process. - @type queue_size: int - @param buff_size: incoming message buffer size in bytes. If - queue_size is set, this should be set to a number greater - than the queue_size times the average message size. Setting - buff_size to a non-default value affects all subscribers to - this topic in this process. - @type buff_size: int - @param tcp_nodelay: if True, request TCP_NODELAY from - publisher. Use of this option is not generally recommended - in most cases as it is better to rely on timestamps in - message data. Setting tcp_nodelay to True enables TCP_NODELAY - for all subscribers in the same python process. - @type tcp_nodelay: bool - @raise ROSException: if parameters are invalid - """ - super(Subscriber, self).__init__(name, data_class, Registration.SUB) - #add in args that factory cannot pass in - - # last person to set these to non-defaults wins, not much way - # around this - if queue_size is not None: - self.impl.set_queue_size(queue_size) - if buff_size != DEFAULT_BUFF_SIZE: - self.impl.set_buff_size(buff_size) - - if callback is not None: - # #1852 - # it's important that we call add_callback so that the - # callback can be invoked with any latched messages - self.impl.add_callback(callback, callback_args) - # save arguments for unregister - self.callback = callback - self.callback_args = callback_args - else: - # initialize fields - self.callback = self.callback_args = None - if tcp_nodelay: - self.impl.set_tcp_nodelay(tcp_nodelay) - - def unregister(self): - """ - unpublish/unsubscribe from topic. Topic instance is no longer - valid after this call. Additional calls to unregister() have no effect. - """ - if self.impl: - # It's possible to have a Subscriber instance with no - # associated callback - if self.callback is not None: - self.impl.remove_callback(self.callback, self.callback_args) - self.callback = self.callback_args = None - super(Subscriber, self).unregister() - -class _SubscriberImpl(_TopicImpl): - """ - Underyling L{_TopicImpl} implementation for subscriptions. - """ - - def __init__(self, name, data_class): - """ - ctor. - @param name: graph resource name of topic, e.g. 'laser'. - @type name: str - @param data_class: Message data class - @type data_class: L{Message} class - """ - super(_SubscriberImpl, self).__init__(name, data_class) - # client-methods to invoke on new messages. should only modify - # under lock. This is a list of 2-tuples (fn, args), where - # args are additional arguments for the callback, or None - self.callbacks = [] - self.queue_size = None - self.buff_size = DEFAULT_BUFF_SIZE - self.tcp_nodelay = False - - def close(self): - """close I/O and release resources""" - _TopicImpl.close(self) - if self.callbacks: - del self.callbacks[:] - self.callbacks = None - - def set_tcp_nodelay(self, tcp_nodelay): - """ - Set the value of TCP_NODELAY, which causes the Nagle algorithm - to be disabled for future topic connections, if the publisher - supports it. - """ - self.tcp_nodelay = tcp_nodelay - - def set_queue_size(self, queue_size): - """ - Set the receive queue size. If more than queue_size messages - are waiting to be deserialized, they are discarded. - - @param queue_size int: incoming queue size. Must be positive integer or None. - @type queue_size: int - """ - if queue_size == -1: - self.queue_size = None - elif queue_size == 0: - raise ROSException("queue size may not be set to zero") - elif queue_size is not None and type(queue_size) != int: - raise ROSException("queue size must be an integer") - else: - self.queue_size = queue_size - - def set_buff_size(self, buff_size): - """ - Set the receive buffer size. The exact meaning of this is - transport dependent. - @param buff_size: receive buffer size - @type buff_size: int - """ - if type(buff_size) != int: - raise ROSException("buffer size must be an integer") - elif buff_size <= 0: - raise ROSException("buffer size must be a positive integer") - self.buff_size = buff_size - - def get_stats(self): # STATS - """ - Get the stats for this topic subscriber - @return: stats for topic in getBusStats() publisher format:: - (topicName, connStats) - where connStats is:: - [connectionId, bytesReceived, numSent, dropEstimate, connected]* - @rtype: list - """ - # save reference to avoid locking - conn = self.connections - dead_conn = self.dead_connections - #for now drop estimate is -1 - stats = (self.resolved_name, - [(c.id, c.stat_bytes, c.stat_num_msg, -1, not c.done) - for c in chain(conn, dead_conn)] ) - return stats - - def add_callback(self, cb, cb_args): - """ - Register a callback to be invoked whenever a new message is received - @param cb: callback function to invoke with message data - instance, i.e. fn(data). If callback args is set, they will - be passed in as the second argument. - @type cb: fn(msg) - @param cb_cargs: additional arguments to pass to callback - @type cb_cargs: Any - """ - if self.closed: - raise ROSException("subscriber [%s] has been closed"%(self.resolved_name)) - with self.c_lock: - # we lock in order to serialize calls to add_callback, but - # we copy self.callbacks so we can it - new_callbacks = self.callbacks[:] - new_callbacks.append((cb, cb_args)) - self.callbacks = new_callbacks - - # #1852: invoke callback with any latched messages - for c in self.connections: - if c.latch is not None: - self._invoke_callback(c.latch, cb, cb_args) - - def remove_callback(self, cb, cb_args): - """ - Unregister a message callback. - @param cb: callback function - @type cb: fn(msg) - @param cb_cargs: additional arguments associated with callback - @type cb_cargs: Any - @raise KeyError: if no matching callback - """ - if self.closed: - return - with self.c_lock: - # we lock in order to serialize calls to add_callback, but - # we copy self.callbacks so we can it - matches = [x for x in self.callbacks if x[0] == cb and x[1] == cb_args] - if matches: - new_callbacks = self.callbacks[:] - # remove the first match - new_callbacks.remove(matches[0]) - self.callbacks = new_callbacks - if not matches: - raise KeyError("no matching cb") - - def _invoke_callback(self, msg, cb, cb_args): - """ - Invoke callback on msg. Traps and logs any exceptions raise by callback - @param msg: message data - @type msg: L{Message} - @param cb: callback - @type cb: fn(msg, cb_args) - @param cb_args: callback args or None - @type cb_args: Any - """ - try: - if cb_args is not None: - cb(msg, cb_args) - else: - cb(msg) - except Exception, e: - if not is_shutdown(): - logerr("bad callback: %s\n%s"%(cb, traceback.format_exc())) - else: - _logger.warn("during shutdown, bad callback: %s\n%s"%(cb, traceback.format_exc())) - - def receive_callback(self, msgs): - """ - Called by underlying connection transport for each new message received - @param msgs: message data - @type msgs: [L{Message}] - """ - # save reference to avoid lock - callbacks = self.callbacks - for msg in msgs: - for cb, cb_args in callbacks: - self._invoke_callback(msg, cb, cb_args) - -class SubscribeListener(object): - """ - Callback API to receive notifications when new subscribers - connect and disconnect. - """ - - def peer_subscribe(self, topic_name, topic_publish, peer_publish): - """ - callback when a peer has subscribed from a topic - @param topic_name: topic name. NOTE: topic name will be resolved/remapped - @type topic_name: str - @param topic_publish: method to publish message data to all subscribers - @type topic_publish: fn(data) - @param peer_publish: method to publish message data to - new subscriber. NOTE: behavior for the latter is - transport-dependent as some transports may be broadcast only. - @type peer_publish: fn(data) - """ - pass - - def peer_unsubscribe(self, topic_name, num_peers): - """ - callback when a peer has unsubscribed from a topic - @param topic_name: topic name. NOTE: topic name will be resolved/remapped - @type topic_name: str - @param num_peers: number of remaining peers subscribed to topic - @type num_peers: int - """ - pass - - -# Implementation note: Publisher attaches to a -# _PublisherImpl singleton for that topic. The underlying impl -# manages the connections for that publication and enables -# thread-safe access - -class Publisher(Topic): - """ - Class for registering as a publisher of a ROS topic. - """ - - def __init__(self, name, data_class, subscriber_listener=None, tcp_nodelay=False, latch=False, headers=None): - """ - Constructor - @param name: resource name of topic, e.g. 'laser'. - @type name: str - @param data_class: message class for serialization - @type data_class: L{Message} class - @param subscriber_listener: listener for - subscription events. May be None. - @type subscriber_listener: L{SubscribeListener} - @param tcp_nodelay: If True, sets TCP_NODELAY on - publisher's socket (disables Nagle algorithm). This results - in lower latency publishing at the cost of efficiency. - @type tcp_nodelay: bool - @param latch: If True, the last message published is - 'latched', meaning that any future subscribers will be sent - that message immediately upon connection. - @type latch: bool - @raise ROSException: if parameters are invalid - """ - super(Publisher, self).__init__(name, data_class, Registration.PUB) - - if subscriber_listener: - self.impl.add_subscriber_listener(subscriber_listener) - if tcp_nodelay: - get_tcpros_handler().set_tcp_nodelay(self.resolved_name, tcp_nodelay) - if latch: - self.impl.enable_latch() - if headers: - self.impl.add_headers(headers) - - def publish(self, *args, **kwds): - """ - Publish message data object to this topic. - Publish can either be called with the message instance to - publish or with the constructor args for a new Message - instance, i.e.:: - pub.publish(message_instance) - pub.publish(message_field_1, message_field_2...) - pub.publish(message_field_1='foo', message_field_2='bar') - - @param args : L{Message} instance, message arguments, or no args if keyword arguments are used - @param kwds : Message keyword arguments. If kwds are used, args must be unset - @raise ROSException: If rospy node has not been initialized - @raise ROSSerializationException: If unable to serialize - message. This is usually a type error with one of the fields. - """ - if self.impl is None: - raise ROSException("publish() to an unregistered() handle") - if not is_initialized(): - raise ROSException("ROS node has not been initialized yet. Please call init_node() first") - data = args_kwds_to_message(self.data_class, args, kwds) - try: - self.impl.acquire() - self.impl.publish(data) - except roslib.message.SerializationError, e: - # can't go to rospy.logerr(), b/c this could potentially recurse - _logger.error(traceback.format_exc(e)) - raise ROSSerializationException(str(e)) - finally: - self.impl.release() - -class _PublisherImpl(_TopicImpl): - """ - Underyling L{_TopicImpl} implementation for publishers. - """ - - def __init__(self, name, data_class): - """ - @param name: name of topic, e.g. 'laser'. - @type name: str - @param data_class: Message data class - @type data_class: L{Message} class - """ - super(_PublisherImpl, self).__init__(name, data_class) - self.buff = cStringIO.StringIO() - self.publock = threading.RLock() #for acquire()/release - self.subscriber_listeners = [] - - # additional client connection headers - self.headers = {} - - # publish latch, starts disabled - self.is_latch = False - self.latch = None - - #STATS - self.message_data_sent = 0 - - def close(self): - """close I/O and release resources""" - _TopicImpl.close(self) - # release resources - if self.subscriber_listeners: - del self.subscriber_listeners[:] - if self.headers: - self.headers.clear() - if self.buff is not None: - self.buff.close() - self.publock = self.headers = self.buff = self.subscriber_listeners = None - - def add_headers(self, headers): - """ - Add connection headers to this Topic for future connections. - @param headers: key/values will be added to current connection - header set, overriding any existing keys if they conflict. - @type headers: dict - """ - self.headers.update(headers) - - def enable_latch(self): - """ - Enable publish() latch. The latch contains the last published - message and is sent to any new subscribers. - """ - self.is_latch = True - - def get_stats(self): # STATS - """ - Get the stats for this topic publisher - @return: stats for topic in getBusStats() publisher format:: - [topicName, messageDataBytes, connStats], - where connStats is:: - [id, bytes, numMessages, connected]* - @rtype: list - """ - # save reference to avoid lock - conn = self.connections - dead_conn = self.dead_connections - return (self.resolved_name, self.message_data_sent, - [(c.id, c.stat_bytes, c.stat_num_msg, not c.done) for c in chain(conn, dead_conn)] ) - - def add_subscriber_listener(self, l): - """ - Add a L{SubscribeListener} for subscribe events. - @param l: listener instance - @type l: L{SubscribeListener} - """ - self.subscriber_listeners.append(l) - - def acquire(self): - """lock for thread-safe publishing to this transport""" - if self.publock is not None: - self.publock.acquire() - - def release(self): - """lock for thread-safe publishing to this transport""" - if self.publock is not None: - self.publock.release() - - def add_connection(self, c): - """ - Add a connection to this topic. This must be a PubTransport. If - the latch is enabled, c will be sent a the value of the - latch. - @param c: connection instance - @type c: L{Transport} - @return: True if connection was added - @rtype: bool - """ - super(_PublisherImpl, self).add_connection(c) - def publish_single(data): - self.publish(data, connection_override=c) - for l in self.subscriber_listeners: - l.peer_subscribe(self.resolved_name, self.publish, publish_single) - if self.is_latch and self.latch is not None: - with self.publock: - self.publish(self.latch, connection_override=c) - return True - - def remove_connection(self, c): - """ - Remove existing connection from this topic. - @param c: connection instance to remove - @type c: L{Transport} - """ - super(_PublisherImpl, self).remove_connection(c) - num = len(self.connections) - for l in self.subscriber_listeners: - l.peer_unsubscribe(self.resolved_name, num) - - def publish(self, message, connection_override=None): - """ - Publish the data to the topic. If the topic has no subscribers, - the method will return without any affect. Access to publish() - should be locked using acquire() and release() in order to - ensure proper message publish ordering. - - @param message: message data instance to publish - @type message: L{Message} - @param connection_override: publish to this connection instead of all - @type connection_override: L{Transport} - @return: True if the data was published, False otherwise. - @rtype: bool - @raise roslib.message.SerializationError: if L{Message} instance is unable to serialize itself - @raise rospy.ROSException: if topic has been closed or was closed during publish() - """ - #TODO: should really just use IOError instead of rospy.ROSException - - if self.closed: - # during shutdown, the topic can get closed, which creates - # a race condition with user code testing is_shutdown - if not is_shutdown(): - raise ROSException("publish() to a closed topic") - else: - return - - if self.is_latch: - self.latch = message - - if not self.has_connections(): - #publish() falls through - return False - - if connection_override is None: - #copy connections so we can iterate safely - conns = self.connections - else: - conns = [connection_override] - - # #2128 test our buffer. I don't now how this got closed in - # that case, but we can at least diagnose the problem. - b = self.buff - try: - b.tell() - - # serialize the message - self.seq += 1 #count messages published to the topic - serialize_message(b, self.seq, message) - - # send the buffer to all connections - err_con = [] - data = b.getvalue() - - for c in conns: - try: - if not is_shutdown(): - c.write_data(data) - except TransportTerminated, e: - logdebug("publisher connection to [%s] terminated, see errorlog for details:\n%s"%(c.endpoint_id, traceback.format_exc())) - err_con.append(c) - except Exception, e: - # greater severity level - logdebug("publisher connection to [%s] terminated, see errorlog for details:\n%s"%(c.endpoint_id, traceback.format_exc())) - err_con.append(c) - - # reset the buffer and update stats - self.message_data_sent += b.tell() #STATS - b.seek(0) - b.truncate(0) - - except ValueError: - # operations on self.buff can fail if topic is closed - # during publish, which often happens during Ctrl-C. - # diagnose the error and report accordingly. - if self.closed: - if is_shutdown(): - # we offer no guarantees on publishes that occur - # during shutdown, so this is not exceptional. - return - else: - # this indicates that user-level code most likely - # closed the topic, which is exceptional. - raise ROSException("topic was closed during publish()") - else: - # unexpected, so re-raise original error - raise - - # remove any bad connections - for c in err_con: - try: - # connection will callback into remove_connection when - # we close it - c.close() - except: - pass - -################################################################################# -# TOPIC MANAGER/LISTENER - -class _TopicManager(object): - """ - Tracks Topic objects - See L{get_topic_manager()} for singleton access - """ - - def __init__(self): - """ctor.""" - super(_TopicManager, self).__init__() - self.pubs = {} #: { topic: _PublisherImpl } - self.subs = {} #: { topic: _SubscriberImpl } - self.topics = set() # [str] list of topic names - self.lock = threading.Condition() - self.closed = False - _logger.info("topicmanager initialized") - - def get_pub_sub_info(self): - """ - get topic publisher and subscriber connection info for getBusInfo() api - @return: [bus info stats] - See getBusInfo() API for more data structure details. - @rtype: list - """ - with self.lock: - info = [] - for s in chain(self.pubs.itervalues(), self.subs.itervalues()): - info.extend(s.get_stats_info()) - return info - - def get_pub_sub_stats(self): - """ - get topic publisher and subscriber stats for getBusStats() api - @return: [publisherStats, subscriberStats]. - See getBusStats() API for more data structure details. - @rtype: list - """ - with self.lock: - return [s.get_stats() for s in self.pubs.itervalues()],\ - [s.get_stats() for s in self.subs.itervalues()] - - def close_all(self): - """ - Close all registered publication and subscriptions. Manager is - no longer usable after close. - """ - self.closed = True - with self.lock: - for t in chain(self.pubs.itervalues(), self.subs.itervalues()): - t.close() - self.pubs.clear() - self.subs.clear() - - def _add(self, ps, rmap, reg_type): - """ - Add L{_TopicImpl} instance to rmap - @param ps: a pub/sub impl instance - @type ps: L{_TopicImpl} - @param rmap: { topic: _TopicImpl} rmap to record instance in - @type rmap: dict - @param reg_type: L{rospy.registration.Registration.PUB} or L{rospy.registration.Registration.SUB} - @type reg_type: str - """ - resolved_name = ps.resolved_name - _logger.debug("tm._add: %s, %s, %s", resolved_name, ps.type, reg_type) - with self.lock: - rmap[resolved_name] = ps - self.topics.add(resolved_name) - - # NOTE: this call can take a lengthy amount of time (at - # least until its reimplemented to use queues) - get_registration_listeners().notify_added(resolved_name, ps.type, reg_type) - - def _recalculate_topics(self): - """recalculate self.topics. expensive""" - self.topics = set([x.resolved_name for x in self.pubs.itervalues()] + - [x.resolved_name for x in self.subs.itervalues()]) - - def _remove(self, ps, rmap, reg_type): - """ - Remove L{_TopicImpl} instance from rmap - @param ps: a pub/sub impl instance - @type ps: L{_TopicImpl} - @param rmap: topic->_TopicImpl rmap to remove instance in - @type rmap: dict - @param reg_type: L{rospy.registration.Registration.PUB} or L{rospy.registration.Registration.SUB} - @type reg_type: str - """ - resolved_name = ps.resolved_name - _logger.debug("tm._remove: %s, %s, %s", resolved_name, ps.type, reg_type) - with self.lock: - del rmap[resolved_name] - self. _recalculate_topics() - - # NOTE: this call can take a lengthy amount of time (at - # least until its reimplemented to use queues) - get_registration_listeners().notify_removed(resolved_name, ps.type, reg_type) - - def get_impl(self, reg_type, resolved_name): - """ - Get the L{_TopicImpl} for the specified topic. This is mainly for - testing purposes. Unlike acquire_impl, it does not alter the - ref count. - @param resolved_name: resolved topic name - @type resolved_name: str - @param reg_type: L{rospy.registration.Registration.PUB} or L{rospy.registration.Registration.SUB} - @type reg_type: str - """ - if reg_type == Registration.PUB: - rmap = self.pubs - elif reg_type == Registration.SUB: - rmap = self.subs - else: - raise TypeError("invalid reg_type: %s"%s) - return rmap.get(resolved_name, None) - - def acquire_impl(self, reg_type, resolved_name, data_class): - """ - Acquire a L{_TopicImpl} for the specified topic (create one if it - doesn't exist). Every L{Topic} instance has a _TopicImpl that - actually controls the topic resources so that multiple Topic - instances use the same underlying connections. 'Acquiring' a - topic implementation marks that another Topic instance is - using the TopicImpl. - - @param resolved_name: resolved topic name - @type resolved_name: str - - @param reg_type: L{rospy.registration.Registration.PUB} or L{rospy.registration.Registration.SUB} - @type reg_type: str - - @param data_class: message class for topic - @type data_class: L{Message} class - """ - if reg_type == Registration.PUB: - rmap = self.pubs - impl_class = _PublisherImpl - elif reg_type == Registration.SUB: - rmap = self.subs - impl_class = _SubscriberImpl - else: - raise TypeError("invalid reg_type: %s"%s) - with self.lock: - impl = rmap.get(resolved_name, None) - if not impl: - impl = impl_class(resolved_name, data_class) - self._add(impl, rmap, reg_type) - impl.ref_count += 1 - return impl - - def release_impl(self, reg_type, resolved_name): - """ - Release a L_{TopicImpl} for the specified topic. - - Every L{Topic} instance has a _TopicImpl that actually - controls the topic resources so that multiple Topic instances - use the same underlying connections. 'Acquiring' a topic - implementation marks that another Topic instance is using the - TopicImpl. - - @param resolved_name: resolved topic name - @type resolved_name: str - @param reg_type: L{rospy.registration.Registration.PUB} or L{rospy.registration.Registration.SUB} - @type reg_type: str - """ - if reg_type == Registration.PUB: - rmap = self.pubs - else: - rmap = self.subs - with self.lock: - # check for race condition where multiple things are cleaning up at once - if self.closed: - return - impl = rmap.get(resolved_name, None) - assert impl is not None, "cannot release topic impl as impl [%s] does not exist"%resolved_name - impl.ref_count -= 1 - assert impl.ref_count >= 0, "topic impl's reference count has gone below zero" - if impl.ref_count == 0: - _logger.debug("topic impl's ref count is zero, deleting topic %s...", resolved_name) - impl.close() - self._remove(impl, rmap, reg_type) - del impl - _logger.debug("... done deleting topic %s", resolved_name) - - def get_publisher_impl(self, resolved_name): - """ - @param resolved_name: resolved topic name - @type resolved_name: str - @return: list of L{_PublisherImpl}s - @rtype: [L{_PublisherImpl}] - """ - return self.pubs.get(resolved_name, None) - - def get_subscriber_impl(self, resolved_name): - """ - @param resolved_name: topic name - @type resolved_name: str - @return: subscriber for the specified topic. - @rtype: L{_SubscriberImpl} - """ - return self.subs.get(resolved_name, None) - - def has_subscription(self, resolved_name): - """ - @param resolved_name: resolved topic name - @type resolved_name: str - @return: True if manager has subscription for specified topic - @rtype: bool - """ - return resolved_name in self.subs - - def has_publication(self, resolved_name): - """ - @param resolved_name: resolved topic name - @type resolved_name: str - @return: True if manager has publication for specified topic - @rtype: bool - """ - return resolved_name in self.pubs - - def get_topics(self): - """ - @return: list of topic names this node subscribes to/publishes - @rtype: [str] - """ - return self.topics - - def _get_list(self, rmap): - return [[k, v.type] for k, v in rmap.iteritems()] - - ## @return [[str,str],]: list of topics subscribed to by this node, [ [topic1, topicType1]...[topicN, topicTypeN]] - def get_subscriptions(self): - return self._get_list(self.subs) - - ## @return [[str,str],]: list of topics published by this node, [ [topic1, topicType1]...[topicN, topicTypeN]] - def get_publications(self): - return self._get_list(self.pubs) - -set_topic_manager(_TopicManager()) -