p36850791
/
kinetic-devel
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

to ros_comm

This commit is contained in:
Tully Foote 2010-10-26 21:27:25 +00:00
parent 57eae4cf0d
commit 0bc139eaad
35 changed files with 0 additions and 8118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
core/rospy/CMakeLists.txt
View File

@ -1,3 +0,0 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()

32
core/rospy/LICENSE
View File

@ -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.

1
core/rospy/Makefile
View File

@ -1 +0,0 @@
include $(shell rospack find mk)/cmake.mk

100
core/rospy/cmake/rospy.cmake
View File

@ -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()

12
core/rospy/epydoc.config
View File

@ -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

39
core/rospy/manifest.xml
View File

@ -1,39 +0,0 @@
<package>
<description brief="ROS Python client library">
<p>
rospy is a pure Python client library for ROS. The rospy client
API enables Python programmers to quickly interface with
ROS <a href="http://ros.org/wiki/Topics">Topics</a>, <a href="http://ros.org/wiki/Services">Services</a>,
and <a href="http://ros.org/wiki/Parameter Server">Parameters</a>. 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.
</p>
<p>
Many of the ROS tools, such
as <a href="http://ros.org/wiki/rostopic">rostopic</a>
and <a href="http://ros.org/wiki/rosservice">rosservice</a>, are
built on top of rospy.
</p>
</description>
<author>Ken Conley/kwc@willowgarage.com</author>
<license>BSD</license>
<review status="Doc reviewed" notes="2010/01/18"/>
<url>http://ros.org/wiki/rospy</url>
<depend package="roslib"/>
<depend package="roslang"/>
<export>
<roslang cmake="${prefix}/cmake/rospy.cmake"/>
<rosdoc config="rosdoc.yaml"/>
</export>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
<platform os="macports" version="macports"/>
</package>

2
core/rospy/rosdoc.yaml
View File

@ -1,2 +0,0 @@
- builder: epydoc
config: epydoc.config

92
core/rospy/scripts/genmsg_py.py
View File

@ -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())

111
core/rospy/scripts/gensrv_py.py
View File

@ -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())

323
core/rospy/scripts/genutil.py
View File

@ -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)

131
core/rospy/src/rospy/__init__.py
View File

@ -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',
]

532
core/rospy/src/rospy/client.py
View File

@ -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")

516
core/rospy/src/rospy/core.py
View File

@ -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]

88
core/rospy/src/rospy/exceptions.py
View File

@ -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

0
core/rospy/src/rospy/impl/__init__.py
View File

147
core/rospy/src/rospy/impl/init.py
View File

@ -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'))

523
core/rospy/src/rospy/impl/masterslave.py
View File

@ -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", []

108
core/rospy/src/rospy/impl/paramserver.py
View File

@ -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

471
core/rospy/src/rospy/impl/registration.py
View File

@ -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()

116
core/rospy/src/rospy/impl/rosout.py
View File

@ -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)

93
core/rospy/src/rospy/impl/simtime.py
View File

@ -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

58
core/rospy/src/rospy/impl/tcpros.py
View File

@ -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

657
core/rospy/src/rospy/impl/tcpros_base.py
View File

@ -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()

317
core/rospy/src/rospy/impl/tcpros_pubsub.py
View File

@ -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)

682
core/rospy/src/rospy/impl/tcpros_service.py
View File

@ -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('<B', b.read(1))[0] # read in ok byte
b.seek(pos)
if not ok:
str = self._read_service_error(sock, b)
#_read_ok_byte has to reset state of the buffer to
#consumed as this exception will bypass rest of
#deserialized_messages logic. we currently can't have
#multiple requests in flight, so we can keep this simple
b.seek(0)
b.truncate(0)
raise ServiceException("service [%s] responded with an error: %s"%(self.resolved_name, str))
else:
# success, set seek point to start of message
b.seek(pos)
def read_messages(self, b, msg_queue, sock):
"""
In service implementation, reads in OK byte that preceeds each
response. The OK byte allows for the passing of error messages
instead of a response message
@param b: buffer
@type b: StringIO
@param msg_queue: Message queue to append to
@type msg_queue: [Message]
@param sock: socket to read from
@type sock: socket.socket
"""
self._read_ok_byte(b, sock)
rospy.msg.deserialize_messages(b, msg_queue, self.recv_data_class, queue_size=self.queue_size, max_msgs=1, start=1) #rospy.msg
#deserialize_messages only resets the buffer to the start
#point if everything was consumed, so we have to further reset
#it.
if b.tell() == 1:
b.seek(0)
def _read_service_error(self, sock, b):
"""
Read service error from sock
@param sock: socket to read from
@type sock: socket
@param b: currently read data from sock
@type b: StringIO
"""
buff_size = 256 #can be small given that we are just reading an error string
while b.tell() < 5:
recv_buff(sock, b, buff_size)
bval = b.getvalue()
(length,) = struct.unpack('<I', bval[1:5]) # ready in len byte
while b.tell() < (5 + length):
recv_buff(sock, b, buff_size)
bval = b.getvalue()
return struct.unpack('<%ss'%length, bval[5:5+length])[0] # ready in len byte
class ServiceProxy(_Service):
"""
Create a handle to a ROS service for invoking calls.
Usage::
add_two_ints = ServiceProxy('add_two_ints', AddTwoInts)
resp = add_two_ints(1, 2)
"""
def __init__(self, name, service_class, persistent=False, headers=None):
"""
ctor.
@param name: name of service to call
@type name: str
@param service_class: auto-generated service class
@type service_class: Service class
@param persistent: (optional) if True, proxy maintains a persistent
connection to service. While this results in better call
performance, persistent connections are discouraged as they are
less resistent to network issues and service restarts.
@type persistent: bool
@param headers: (optional) arbitrary headers
@type headers: dict
"""
super(ServiceProxy, self).__init__(name, service_class)
self.uri = None
self.seq = 0
self.buff_size = DEFAULT_BUFF_SIZE
self.persistent = persistent
if persistent:
if not headers:
headers = {}
headers['persistent'] = '1'
self.protocol = TCPROSServiceClient(self.resolved_name,
self.service_class, headers=headers)
self.transport = None #for saving persistent connections
def wait_for_service(self, timeout=None):
wait_for_service(self.resolved_name, timeout=timeout)
# #425
def __call__(self, *args, **kwds):
"""
Callable-style version of the service API. This accepts either a request message instance,
or you can call directly with arguments to create a new request instance. e.g.::
add_two_ints(AddTwoIntsRequest(1, 2))
add_two_ints(1, 2)
add_two_ints(a=1, b=2)
@param args: arguments to remote service
@param kwds: message keyword arguments
@raise ROSSerializationException: If unable to serialize
message. This is usually a type error with one of the fields.
"""
return self.call(*args, **kwds)
def _get_service_uri(self, request):
"""
private routine for getting URI of service to call
@param request: request message
@type request: L{rospy.Message}
"""
if not isinstance(request, roslib.message.Message):
raise TypeError("request object is not a valid request message instance")
# in order to support more interesting overrides, we only
# check that it declares the same ROS type instead of a
# stricter class check
#if not self.request_class == request.__class__:
if not self.request_class._type == request._type:
raise TypeError("request object type [%s] does not match service type [%s]"%(request.__class__, self.request_class))
#TODO: subscribe to service changes
#if self.uri is None:
if 1: #always do lookup for now, in the future we need to optimize
try:
try:
code, msg, self.uri = roslib.scriptutil.get_master().lookupService(rospy.names.get_caller_id(), self.resolved_name)
except:
raise ServiceException("unable to contact master")
if code != 1:
logger.error("[%s]: lookup service failed with message [%s]", self.resolved_name, msg)
raise ServiceException("service [%s] unavailable"%self.resolved_name)
# validate
try:
rospy.core.parse_rosrpc_uri(self.uri)
except rospy.impl.validators.ParameterInvalid:
raise ServiceException("master returned invalid ROSRPC URI: %s"%self.uri)
except socket.error, e:
logger.error("[%s]: socket error contacting service, master is probably unavailable",self.resolved_name)
return self.uri
def call(self, *args, **kwds):
"""
Call the service. This accepts either a request message instance,
or you can call directly with arguments to create a new request instance. e.g.::
add_two_ints(AddTwoIntsRequest(1, 2))
add_two_ints(1, 2)
add_two_ints(a=1, b=2)
@raise TypeError: if request is not of the valid type (Message)
@raise ServiceException: if communication with remote service fails
@raise ROSInterruptException: if node shutdown (e.g. ctrl-C) interrupts service call
@raise ROSSerializationException: If unable to serialize
message. This is usually a type error with one of the fields.
"""
# convert args/kwds to request message class
request = rospy.msg.args_kwds_to_message(self.request_class, args, kwds)
# initialize transport
if self.transport is None:
service_uri = self._get_service_uri(request)
dest_addr, dest_port = rospy.core.parse_rosrpc_uri(service_uri)
# connect to service
transport = TCPROSTransport(self.protocol, self.resolved_name)
transport.buff_size = self.buff_size
try:
transport.connect(dest_addr, dest_port, service_uri)
except TransportInitError, e:
# can be a connection or md5sum mismatch
raise ServiceException("unable to connect to service: %s"%e)
self.transport = transport
else:
transport = self.transport
# send the actual request message
self.seq += 1
transport.send_message(request, self.seq)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise ServiceException("service [%s] returned no response"%self.resolved_name)
elif len(responses) > 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('<BI%ss'%len(err_msg), 0, len(err_msg), err_msg))
def _handle_request(self, transport, request):
"""
Process a single incoming request.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param request: Message
@type request: roslib.message.Message
"""
try:
# convert return type to response Message instance
response = convert_return_to_response(self.handler(request), self.response_class)
self.seq += 1
# ok byte
transport.write_buff.write(struct.pack('<B', 1))
transport.send_message(response, self.seq)
except ServiceException, e:
rospy.core.rospydebug("handler raised ServiceException: %s"%(e))
self._write_service_error(transport, "service cannot process request: %s"%e)
except Exception, e:
logerr("Error processing request: %s\n%s"%(e,traceback.print_exc()))
self._write_service_error(transport, "error processing request: %s"%e)
def handle(self, transport, header):
"""
Process incoming request. This method should be run in its
own thread. If header['persistent'] is set to 1, method will
block until connection is broken.
@param transport: transport instance
@type transport: L{TCPROSTransport}
@param header: headers from client
@type header: dict
"""
if 'persistent' in header and \
header['persistent'].lower() in ['1', 'true']:
persistent = True
else:
persistent = False
if header.get('probe', None) == '1':
#this will likely do more in the future
transport.close()
return
handle_done = False
while not handle_done:
try:
requests = transport.receive_once()
for request in requests:
self._handle_request(transport, request)
if not persistent:
handle_done = True
except rospy.exceptions.TransportTerminated, e:
if not persistent:
logerr("incoming connection failed: %s"%e)
logdebug("service[%s]: transport terminated"%self.resolved_name)
handle_done = True
transport.close()
class Service(ServiceImpl):
"""
Declare a ROS service. Service requests are passed to the
specified handler.
Service Usage::
s = Service('getmapservice', GetMap, get_map_handler)
"""
def __init__(self, name, service_class, handler, buff_size=DEFAULT_BUFF_SIZE):
"""
ctor.
@param name: service name
@type name: str
@param service_class: ServiceDefinition class
@type service_class: ServiceDefinition class
@param handler: callback function for processing service
request. Function takes in a ServiceRequest and returns a
ServiceResponse of the appropriate type. Function may also
return a list, tuple, or dictionary with arguments to initialize
a ServiceResponse instance of the correct type.
If handler cannot process request, it may either return None,
to indicate failure, or it may raise a rospy.ServiceException
to send a specific error message to the client. Returning None
is always considered a failure.
@type handler: fn(req)->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)

190
core/rospy/src/rospy/impl/transport.py
View File

@ -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")

300
core/rospy/src/rospy/impl/udpros.py
View File

@ -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

51
core/rospy/src/rospy/impl/validators.py
View File

@ -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

245
core/rospy/src/rospy/msg.py
View File

@ -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('<I', size))
b.seek(end)
def deserialize_messages(b, msg_queue, data_class, queue_size=None, max_msgs=None, start=0):
"""
Read all messages off the buffer
@param b: buffer to read data from
@type b: StringIO
@param msg_queue: queue to append deserialized data to
@type msg_queue: list
@param data_class: message deserialization class
@type data_class: Message class
@param queue_size: message queue size. all but the last
queue_size messages are discarded if this parameter is specified.
@type queue_size: int
@param start: starting position to read in b
@type start: int
@param max_msgs int: maximum number of messages to deserialize or None
@type max_msgs: int
@raise roslib.message.DeserializationError: if an error/exception occurs during deserialization
"""
try:
pos = start
btell = b.tell()
left = btell - pos
# check to see if we even have a message
if left < 4:
return
# read in each message from the buffer as a string. each
# message is preceded by a 4-byte integer length. the
# serialized messages are appended to buff.
b.seek(pos)
buffs = []
# size of message
size = -1
while (size < 0 and left >= 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('<I', b.read(4))
left -= 4
# - deserialize the complete buffer
if size > -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))

197
core/rospy/src/rospy/msproxy.py
View File

@ -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)

328
core/rospy/src/rospy/names.py
View File

@ -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)

93
core/rospy/src/rospy/numpy_msg.py
View File

@ -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)

272
core/rospy/src/rospy/rostime.py
View File

@ -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()

147
core/rospy/src/rospy/service.py
View File

@ -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())

1141
core/rospy/src/rospy/topics.py
View File

File diff suppressed because it is too large Load Diff