pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

222
thirdparty/ros/ros_comm/clients/rospy/CHANGELOG.rst vendored Normal file
View File

@@ -0,0 +1,222 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rospy
^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.12.14 (2018-08-23)
--------------------
* fix some errors in some probably not frequented code paths (`#1415 <https://github.com/ros/ros_comm/issues/1415>`_)
* fix thread problem with get_topics() (`#1416 <https://github.com/ros/ros_comm/issues/1416>`_)
1.12.13 (2018-02-21)
--------------------
* raise the correct exception from AnyMsg.serialize (`#1311 <https://github.com/ros/ros_comm/issues/1311>`_)
1.12.12 (2017-11-16)
--------------------
1.12.11 (2017-11-07)
--------------------
1.12.10 (2017-11-06)
--------------------
1.12.9 (2017-11-06)
-------------------
1.12.8 (2017-11-06)
-------------------
* change rospy.Rate hz type from int to float (`#1177 <https://github.com/ros/ros_comm/issues/1177>`_)
* improve rospy.logXXX_throttle performance (`#1091 <https://github.com/ros/ros_comm/pull/1091>`_)
* add option to reset timer when time moved backwards (`#1083 <https://github.com/ros/ros_comm/issues/1083>`_)
* abort topic lookup on connection refused (`#1044 <https://github.com/ros/ros_comm/pull/1044>`_)
* sleep in rospy wait_for_service even if exceptions raised (`#1025 <https://github.com/ros/ros_comm/pull/1025>`_)
1.12.7 (2017-02-17)
-------------------
* make get_published_topics threadsafe (`#958 <https://github.com/ros/ros_comm/issues/958>`_)
* use poll in write_header() if available to support higher numbered fileno (`#929 <https://github.com/ros/ros_comm/pull/929>`_)
* use epoll instead of poll if available to gracefully close hung connections (`#831 <https://github.com/ros/ros_comm/issues/831>`_)
* fix Python 3 compatibility issues (`#565 <https://github.com/ros/ros_comm/issues/565>`_)
1.12.6 (2016-10-26)
-------------------
* improve reconnection logic on timeout and other common errors (`#851 <https://github.com/ros/ros_comm/pull/851>`_)
* remove duplicated function (`#783 <https://github.com/ros/ros_comm/pull/783>`_)
1.12.5 (2016-09-30)
-------------------
1.12.4 (2016-09-19)
-------------------
1.12.3 (2016-09-17)
-------------------
* raise error on rospy.init_node with None or empty node name string (`#895 <https://github.com/ros/ros_comm/pull/895>`_)
* fix wrong type in docstring for rospy.Timer (`#878 <https://github.com/ros/ros_comm/pull/878>`_)
* fix order of init and publisher in example (`#873 <https://github.com/ros/ros_comm/pull/873>`_)
1.12.2 (2016-06-03)
-------------------
* add logXXX_throttle functions (`#812 <https://github.com/ros/ros_comm/pull/812>`_)
1.12.1 (2016-04-18)
-------------------
1.12.0 (2016-03-18)
-------------------
1.11.18 (2016-03-17)
--------------------
1.11.17 (2016-03-11)
--------------------
* preserve identity of numpy_msg(T) (`#758 <https://github.com/ros/ros_comm/pull/758>`_)
1.11.16 (2015-11-09)
--------------------
* catch ROSInterruptException from rospy timers when shutting down (`#690 <https://github.com/ros/ros_comm/pull/690>`_)
1.11.15 (2015-10-13)
--------------------
* validate name after remapping (`#669 <https://github.com/ros/ros_comm/pull/669>`_)
1.11.14 (2015-09-19)
--------------------
* fix memory/thread leak with QueuedConnection (`#661 <https://github.com/ros/ros_comm/pull/661>`_)
* fix signaling already shutdown to client hooks with the appropriate signature (`#651 <https://github.com/ros/ros_comm/issues/651>`_)
* fix bug with missing current logger levels (`#631 <https://github.com/ros/ros_comm/pull/631>`_)
1.11.13 (2015-04-28)
--------------------
1.11.12 (2015-04-27)
--------------------
1.11.11 (2015-04-16)
--------------------
* add rosconsole command line tool to change logger levels (`#576 <https://github.com/ros/ros_comm/pull/576>`_)
* add accessor for remaining time of the Rate class (`#588 <https://github.com/ros/ros_comm/pull/588>`_)
* fix high latency when using asynchronous publishing (`#547 <https://github.com/ros/ros_comm/issues/547>`_)
* fix error handling when publishing on Empty topic (`#566 <https://github.com/ros/ros_comm/pull/566>`_)
1.11.10 (2014-12-22)
--------------------
* add specific exception for time jumping backwards (`#485 <https://github.com/ros/ros_comm/issues/485>`_)
* make param functions thread-safe (`#523 <https://github.com/ros/ros_comm/pull/523>`_)
* fix infinitely retrying subscriber (`#533 <https://github.com/ros/ros_comm/issues/533>`_)
* fix removal of QueuedConnection leading to wrong subscriber count (`#526 <https://github.com/ros/ros_comm/issues/526>`_)
* fix TCPROS header validation when `callerid` header is not set (`#522 <https://github.com/ros/ros_comm/issues/522>`_, regression from 1.11.1)
* fix memory leak when using subcriber statistics (`#520 <https://github.com/ros/ros_comm/issues/520>`_)
* fix reported traffic in bytes from Python nodes (`#501 <https://github.com/ros/ros_comm/issues/501>`_)
1.11.9 (2014-08-18)
-------------------
* populate delivered_msgs field of TopicStatistics message (`#486 <https://github.com/ros/ros_comm/issues/486>`_)
1.11.8 (2014-08-04)
-------------------
* fix topic/connection statistics reporting code (`#482 <https://github.com/ros/ros_comm/issues/482>`_)
1.11.7 (2014-07-18)
-------------------
1.11.6 (2014-07-10)
-------------------
* make MasterProxy thread-safe (`#459 <https://github.com/ros/ros_comm/issues/459>`_)
* check ROS_HOSTNAME for localhost / ROS_IP for 127./::1 and prevent connections from other hosts in that case (`#452 <https://github.com/ros/ros_comm/issues/452>`)_
1.11.5 (2014-06-24)
-------------------
1.11.4 (2014-06-16)
-------------------
* Python 3 compatibility (`#426 <https://github.com/ros/ros_comm/issues/426>`_)
1.11.3 (2014-05-21)
-------------------
* allow shutdown hooks to be any callable object (`#410 <https://github.com/ros/ros_comm/issues/410>`_)
* add demux program and related scripts (`#407 <https://github.com/ros/ros_comm/issues/407>`_)
* add publisher queue_size to rostopic
1.11.2 (2014-05-08)
-------------------
* use publisher queue_size for statistics (`#398 <https://github.com/ros/ros_comm/issues/398>`_)
1.11.1 (2014-05-07)
-------------------
* improve asynchonous publishing performance (`#373 <https://github.com/ros/ros_comm/issues/373>`_)
* add warning when queue_size is omitted for rospy publisher (`#346 <https://github.com/ros/ros_comm/issues/346>`_)
* add optional topic/connection statistics (`#398 <https://github.com/ros/ros_comm/issues/398>`_)
* add transport information in SlaveAPI::getBusInfo() for roscpp & rospy (`#328 <https://github.com/ros/ros_comm/issues/328>`_)
* allow custom error handlers for services (`#375 <https://github.com/ros/ros_comm/issues/375>`_)
* add architecture_independent flag in package.xml (`#391 <https://github.com/ros/ros_comm/issues/391>`_)
1.11.0 (2014-03-04)
-------------------
* fix exception handling for queued connections (`#369 <https://github.com/ros/ros_comm/issues/369>`_)
* use catkin_install_python() to install Python scripts (`#361 <https://github.com/ros/ros_comm/issues/361>`_)
1.10.0 (2014-02-11)
-------------------
1.9.54 (2014-01-27)
-------------------
1.9.53 (2014-01-14)
-------------------
1.9.52 (2014-01-08)
-------------------
1.9.51 (2014-01-07)
-------------------
* implement optional queueing for rospy publications (`#169 <https://github.com/ros/ros_comm/issues/169>`_)
* overwrite __repr__ for rospy.Duration and Time (`ros/genpy#24 <https://github.com/ros/genpy/issues/24>`_)
* add missing dependency on roscpp
1.9.50 (2013-10-04)
-------------------
* add support for python coverage tool to work in callbacks
1.9.49 (2013-09-16)
-------------------
1.9.48 (2013-08-21)
-------------------
* make rospy nodes killable while waiting for master (`#262 <https://github.com/ros/ros_comm/issues/262>`_)
1.9.47 (2013-07-03)
-------------------
1.9.46 (2013-06-18)
-------------------
1.9.45 (2013-06-06)
-------------------
* add missing run_depend on python-yaml
* allow configuration of ports for XML RPCs and TCP ROS
* fix race condition where rospy subscribers do not connect to all publisher
* fix closing and deregistering connection when connect fails (`#128 <https://github.com/ros/ros_comm/issues/128>`_)
* fix log level of RosOutHandler (`#210 <https://github.com/ros/ros_comm/issues/210>`_)
1.9.44 (2013-03-21)
-------------------
1.9.43 (2013-03-13)
-------------------
1.9.42 (2013-03-08)
-------------------
* make dependencies on rospy optional by refactoring RosStreamHandler to rosgraph (`#179 <https://github.com/ros/ros_comm/issues/179>`_)
1.9.41 (2013-01-24)
-------------------
1.9.40 (2013-01-13)
-------------------
* add colorization for rospy log output (`#3691 <https://code.ros.org/trac/ros/ticket/3691>`_)
* fix socket polling under Windows (`#3959 <https://code.ros.org/trac/ros/ticket/3959>`_)
1.9.39 (2012-12-29)
-------------------
* first public release for Groovy

14
thirdparty/ros/ros_comm/clients/rospy/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.8.3)
project(rospy)
find_package(catkin REQUIRED)
catkin_package()
catkin_python_setup()
# install legacy infrastructure needed by rosbuild
install(FILES rosbuild/rospy.cmake
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rosbuild)
catkin_install_python(PROGRAMS
rosbuild/scripts/genmsg_py.py
rosbuild/scripts/gensrv_py.py
rosbuild/scripts/genutil.py
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rosbuild/scripts)

12
thirdparty/ros/ros_comm/clients/rospy/epydoc.config vendored Normal file
View File

@@ -0,0 +1,12 @@
[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

44
thirdparty/ros/ros_comm/clients/rospy/package.xml vendored Normal file
View File

@@ -0,0 +1,44 @@
<package>
<name>rospy</name>
<version>1.12.14</version>
<description>
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.
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.
</description>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/rospy</url>
<author>Ken Conley</author>
<buildtool_depend version_gte="0.5.78">catkin</buildtool_depend>
<run_depend>genpy</run_depend>
<run_depend>python-numpy</run_depend>
<run_depend>python-rospkg</run_depend>
<run_depend>python-yaml</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rosgraph</run_depend>
<run_depend version_gte="1.10.3">rosgraph_msgs</run_depend>
<run_depend>roslib</run_depend>
<run_depend>std_msgs</run_depend>
<export>
<rosdoc config="rosdoc.yaml"/>
<architecture_independent/>
</export>
</package>

110
thirdparty/ros/ros_comm/clients/rospy/rosbuild/rospy.cmake vendored Normal file
View File

@@ -0,0 +1,110 @@
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}/rosbuild/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)
# Also set up to clean the src/<project>/msg directory
get_directory_property(_old_clean_files ADDITIONAL_MAKE_CLEAN_FILES)
list(APPEND _old_clean_files ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}/msg)
set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${_old_clean_files}")
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}/rosbuild/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)
# Also set up to clean the src/<project>/srv directory
get_directory_property(_old_clean_files ADDITIONAL_MAKE_CLEAN_FILES)
list(APPEND _old_clean_files ${PROJECT_SOURCE_DIR}/src/${PROJECT_NAME}/srv)
set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${_old_clean_files}")
endif(_autogen)
endmacro(gensrv_py)
# Call the macro we just defined.
gensrv_py()

47
thirdparty/ros/ros_comm/clients/rospy/rosbuild/scripts/genmsg_py.py vendored Executable file
View File

@@ -0,0 +1,47 @@
#!/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.
"""
ROS message source code generation for Python
Converts ROS .msg files in a package into Python source code implementations.
"""
import sys
# genutil is a utility package the implements the package crawling
# logic of genmsg_py and gensrv_py logic
import genutil
if __name__ == "__main__":
genutil.genmain(sys.argv, 'msg')

46
thirdparty/ros/ros_comm/clients/rospy/rosbuild/scripts/gensrv_py.py vendored Executable file
View File

@@ -0,0 +1,46 @@
#!/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.
"""
ROS message source code generation for rospy.
Converts ROS .srv files into Python source code implementations.
"""
import sys
# genutil is a utility package the implements the package crawling
# logic of genmsg_py and gensrv_py logic
import genutil
if __name__ == "__main__":
genutil.genmain(sys.argv, 'srv')

143
thirdparty/ros/ros_comm/clients/rospy/rosbuild/scripts/genutil.py vendored Normal file
View File

@@ -0,0 +1,143 @@
# 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.
## Common generation tools for Python ROS message and service generators
from __future__ import print_function
import os
import errno # for smart handling of exceptions for os.makedirs()
import sys
import traceback
import rospkg
import genmsg
import genpy
import genpy.generator
import genpy.generate_initpy
def usage(progname):
print("%(progname)s file(s)"%vars())
def get_package_and_file(argv):
if not argv[1:]:
usage(argv[0])
files = [a for a in argv[1:] if not a.startswith('--')]
# rospy.cmake only passes in a single file arg, assert this case
assert len(files) == 1, files
msg_file = files[0]
package = rospkg.get_package_name(msg_file)
return package, msg_file
def get_outdir(package, path, subdir):
"compute the directory that the .py files are output to"
outdir = os.path.join(path, 'src', package, subdir)
if not os.path.exists(outdir):
try:
os.makedirs(outdir)
except Exception as 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 IOError("Cannot write to %s: file in the way"%(outdir))
return outdir
def generate_messages(rospack, package, msg_file, subdir):
if subdir == 'msg':
gen = genpy.generator.MsgGenerator()
else:
gen = genpy.generator.SrvGenerator()
path = rospack.get_path(package)
search_path = {
package: [os.path.join(path, 'msg')]
}
# std_msgs is implicit depend due to Header
search_path['std_msgs'] = [os.path.join(rospack.get_path('std_msgs'), 'msg')]
for d in rospack.get_depends(package):
search_path[d] = [os.path.join(rospack.get_path(d), 'msg')]
include_args = []
for d, ipaths in search_path.items():
for ipath in ipaths:
include_args.append('-I%s:%s'%(d, ipath))
outdir = get_outdir(package, path, subdir)
retcode = gen.generate_messages(package, [msg_file], outdir, search_path)
return retcode
def generate_initpy(rospack, p, subdir):
path = rospack.get_path(p)
outdir = get_outdir(p, path, subdir)
retcode = genpy.generate_initpy.write_modules(outdir)
parent_initpy = os.path.join(path, 'src', p, '__init__.py')
if not os.path.exists(parent_initpy):
with open(parent_initpy, 'w') as f:
f.write("#autogenerated by ROS python message generators")
return retcode
def genmain(argv, subdir):
rospack = rospkg.RosPack()
try:
gen_initpy = '--initpy' in argv
no_gen_initpy = '--noinitpy' in argv
if gen_initpy:
# #1827
# new __init__py does not take explicit file args. just
# convert to unique package names and compute path.
files = [f for f in argv[1:] if not f.startswith('--')]
packages = list(set([rospkg.get_package_name(f) for f in files]))
retcodes = [generate_initpy(rospack, p, subdir) for p in packages]
retcodes = [c for c in retcodes if c not in (0, None)]
if retcodes:
retcode = retcodes[0]
else:
retcode = 0
else:
package, msg_file = get_package_and_file(argv)
retcode = generate_messages(rospack, package, msg_file, subdir)
except genmsg.InvalidMsgSpec as e:
sys.stderr.write("ERROR: %s\n"%(str(e)))
retcode = 1
except genmsg.MsgGenerationException as e:
sys.stderr.write("ERROR: %s\n"%(str(e)))
retcode = 2
except Exception as e:
traceback.print_exc()
sys.stderr.write("ERROR: %s\n"%(str(e)))
retcode = 3
sys.exit(retcode or 0)

2
thirdparty/ros/ros_comm/clients/rospy/rosdoc.yaml vendored Normal file
View File

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

3
thirdparty/ros/ros_comm/clients/rospy/scripts/rosconsole vendored Executable file
View File

@@ -0,0 +1,3 @@
#!/usr/bin/env python
from rospy import rosconsole
rosconsole.main()

13
thirdparty/ros/ros_comm/clients/rospy/setup.py vendored Normal file
View File

@@ -0,0 +1,13 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['rospy', 'rospy.impl'],
package_dir={'': 'src'},
scripts=['scripts/rosconsole'],
requires=['genpy', 'numpy', 'rosgraph', 'roslib', 'rospkg']
)
setup(**d)

129
thirdparty/ros/ros_comm/clients/rospy/src/rospy/__init__.py vendored Normal file
View File

@@ -0,0 +1,129 @@
# 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)
"""
# 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 std_msgs.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,\
DEBUG, INFO, WARN, ERROR, FATAL
from .timer import sleep, Rate, Timer
from .core import is_shutdown, signal_shutdown, \
get_node_uri, get_ros_root, \
logdebug, logwarn, loginfo, logout, logerr, logfatal, \
logdebug_throttle, logwarn_throttle, loginfo_throttle, logerr_throttle, logfatal_throttle, \
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
# - 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
__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',
'logdebug_throttle',
'logwarn_throttle', 'loginfo_throttle',
'logerr_throttle', 'logfatal_throttle',
'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',
'Service', 'ServiceProxy',
'SubscribeListener', 'Publisher', 'Subscriber',
]

556
thirdparty/ros/ros_comm/clients/rospy/src/rospy/client.py vendored Normal file
View File

@@ -0,0 +1,556 @@
# 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
from threading import Lock
import time
import random
import yaml
import rosgraph
import rosgraph.names
import roslib
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
from rosgraph_msgs.msg import Log
from roscpp.srv import GetLoggers, GetLoggersResponse, SetLoggerLevel, SetLoggerLevelResponse
from roscpp.msg import Logger
from rospy.impl.tcpros_service import Service
DEBUG = Log.DEBUG
INFO = Log.INFO
WARN = Log.WARN
ERROR = Log.ERROR
FATAL = Log.FATAL
def myargv(argv=None):
"""
Remove ROS remapping arguments from sys.argv arguments.
@return: copy of sys.argv with ROS remapping arguments removed
@rtype: [str]
"""
if argv is None:
argv = sys.argv
return [a for a in argv if not rosgraph.names.REMAP in a]
def load_command_line_node_params(argv):
"""
Load node param mappings (aka private parameters) encoded in
command-line arguments, e.g. _foo:=bar. See also rosgraph.names.load_mappings.
@param argv: command-line arguments
@param argv: [str]
@return: param->value remappings.
@rtype: {str: val}
@raises: ROSInitException
"""
try:
mappings = {}
for arg in argv:
if rosgraph.names.REMAP in arg:
src, dst = [x.strip() for x in arg.split(rosgraph.names.REMAP)]
if src and dst:
if len(src) > 1 and src[0] == '_' and src[1] != '_':
mappings[src[1:]] = yaml.load(dst)
return mappings
except Exception as e:
raise rospy.exceptions.ROSInitException("invalid command-line parameters: %s"%(str(e)))
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():
rospy.rostime.wallsleep(0.5)
except KeyboardInterrupt:
logdebug("keyboard interrupt, shutting down")
rospy.core.signal_shutdown('keyboard interrupt')
_logging_level_names = {
logging.DEBUG: 'DEBUG',
logging.INFO: 'INFO',
logging.WARNING: 'WARN',
logging.ERROR: 'ERROR',
logging.CRITICAL: 'FATAL',
}
def _get_loggers(request):
"""
ROS service handler to get the levels of all active loggers.
"""
ret = GetLoggersResponse()
for n in logging.Logger.manager.loggerDict.keys():
level = logging.getLogger(n).getEffectiveLevel()
level = _logging_level_names[level]
ret.loggers.append(Logger(n, level))
return ret
_names_to_logging_levels = {
'DEBUG': logging.DEBUG,
'INFO': logging.INFO,
'WARN': logging.WARNING,
'ERROR': logging.ERROR,
'FATAL': logging.CRITICAL,
}
def _set_logger_level(request):
"""
ROS service handler to set the logging level for a particular logger
"""
level = request.level.upper()
if level in _names_to_logging_levels:
logger = logging.getLogger(request.logger)
logger.setLevel(_names_to_logging_levels[level])
else:
logging.getLogger('rospy').error("Bad logging level: %s"%level)
ret = SetLoggerLevelResponse()
return ret
def _init_node_params(argv, node_name):
"""
Uploads private params to the parameter server. Private params are specified
via command-line remappings.
@raises: ROSInitException
"""
# #1027: load in param name mappings
params = load_command_line_node_params(argv)
for param_name, param_value in params.items():
logdebug("setting param %s to %s"%(param_name, param_value))
set_param(rosgraph.names.PRIV_NAME + param_name, param_value)
_init_node_args = None
def init_node(name, argv=None, anonymous=False, log_level=None, disable_rostime=False, disable_rosout=False, disable_signals=False, xmlrpc_port=0, tcpros_port=0):
"""
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 internal testing only: suppresses
automatic subscription to rostime
@type disable_rostime: bool
@param disable_rosout: for internal testing only: suppress
auto-publication of rosout
@type disable_rostime: bool
@param xmlrpc_port: If provided, it will use this port number for the client
XMLRPC node.
@type xmlrpc_port: int
@param tcpros_port: If provided, the TCPROS server will listen for
connections on this port
@type tcpros_port: int
@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)
if not name:
raise ValueError("name must not be empty")
# this test can be eliminated once we change from warning to error in the next check
if rosgraph.names.SEP in name:
raise ValueError("namespaces are not allowed in node names")
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:
name = mappings['__name']
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))
# check for legal base name once all changes have been made to the name
if not rosgraph.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)
# use rosgraph version of resolve_name to avoid remapping
resolved_node_name = rosgraph.names.resolve_name(name, rospy.core.get_caller_id())
rospy.core.configure_logging(resolved_node_name)
# #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, port=xmlrpc_port, tcpros_port=tcpros_port)
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)
if not disable_rosout:
rospy.impl.rosout.init_rosout()
rospy.impl.rosout.load_rosout_handlers(log_level)
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)
logdebug("init_node, name[%s], pid[%s]", resolved_node_name, os.getpid())
# advertise logging level services
Service('~get_loggers', GetLoggers, _get_loggers)
Service('~set_logger_level', SetLoggerLevel, _set_logger_level)
#_master_proxy is a MasterProxy wrapper
_master_proxy = None
_master_proxy_lock = Lock()
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, _master_proxy_lock
if _master_proxy is None:
with _master_proxy_lock:
if _master_proxy is None:
_master_proxy = rospy.msproxy.MasterProxy(
rosgraph.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:
rospy.rostime.wallsleep(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:
rospy.rostime.wallsleep(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
_param_server_lock = Lock()
def _init_param_server():
"""
Initialize parameter server singleton
"""
global _param_server, _param_server_lock
if _param_server is None:
with _param_server_lock:
if _param_server is None:
_param_server = get_master()
return _param_server_lock
# 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
NOTE: this method is thread-safe.
@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.
NOTE: this method is thread-safe.
@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
NOTE: this method is thread-safe.
If param_value is a dictionary it will be treated as a parameter
tree, where param_name is the namespace. For example:::
{'x':1,'y':2,'sub':{'z':3}}
will set param_name/x=1, param_name/y=2, and param_name/sub/z=3.
Furthermore, it will replace all existing parameters in the
param_name namespace with the parameters in param_value. You must
set parameters individually if you wish to perform a union update.
@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 rosgraph.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
NOTE: this method is thread-safe.
@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
NOTE: this method is thread-safe.
@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
NOTE: this method is thread-safe.
@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

544
thirdparty/ros/ros_comm/clients/rospy/src/rospy/core.py vendored Normal file
View File

@@ -0,0 +1,544 @@
# 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"""
import atexit
try:
import cPickle as pickle
except ImportError:
import pickle
import inspect
import logging
import os
import signal
import sys
import threading
import time
import traceback
import types
try:
import urllib.parse as urlparse #Python 3.x
except ImportError:
import urlparse
try:
import xmlrpc.client as xmlrpcclient #Python 3.x
except ImportError:
import xmlrpclib as xmlrpcclient #Python 2.x
import rospkg
import rosgraph.roslogging
import rospy.exceptions
import rospy.rostime
from rospy.names import *
from rospy.impl.validators import ParameterInvalid
from rosgraph_msgs.msg import Log
_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")
# 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 rospyinfo(msg, *args):
"""Internal rospy client library debug logging"""
_rospy_logger.info(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)
logdebug = logging.getLogger('rosout').debug
logwarn = logging.getLogger('rosout').warning
loginfo = logging.getLogger('rosout').info
logout = loginfo # alias deprecated name
logerr = logging.getLogger('rosout').error
logerror = logerr # alias logerr
logfatal = logging.getLogger('rosout').critical
class LoggingThrottle(object):
last_logging_time_table = {}
def __call__(self, caller_id, logging_func, period, msg):
"""Do logging specified message periodically.
- caller_id (str): Id to identify the caller
- logging_func (function): Function to do logging.
- period (float): Period to do logging in second unit.
- msg (object): Message to do logging.
"""
now = rospy.Time.now()
last_logging_time = self.last_logging_time_table.get(caller_id)
if (last_logging_time is None or
(now - last_logging_time) > rospy.Duration(period)):
logging_func(msg)
self.last_logging_time_table[caller_id] = now
_logging_throttle = LoggingThrottle()
def _frame_to_caller_id(frame):
caller_id = (
inspect.getabsfile(frame),
frame.f_lineno,
frame.f_lasti,
)
return pickle.dumps(caller_id)
def logdebug_throttle(period, msg):
caller_id = _frame_to_caller_id(inspect.currentframe().f_back)
_logging_throttle(caller_id, logdebug, period, msg)
def loginfo_throttle(period, msg):
caller_id = _frame_to_caller_id(inspect.currentframe().f_back)
_logging_throttle(caller_id, loginfo, period, msg)
def logwarn_throttle(period, msg):
caller_id = _frame_to_caller_id(inspect.currentframe().f_back)
_logging_throttle(caller_id, logwarn, period, msg)
def logerr_throttle(period, msg):
caller_id = _frame_to_caller_id(inspect.currentframe().f_back)
_logging_throttle(caller_id, logerr, period, msg)
def logfatal_throttle(period, msg):
caller_id = _frame_to_caller_id(inspect.currentframe().f_back)
_logging_throttle(caller_id, logfatal, period, msg)
#########################################################
# CONSTANTS
MASTER_NAME = "master" #master is a reserved node name for the central master
import warnings
import functools
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."""
@functools.wraps(func)
def newFunc(*args, **kwargs):
warnings.warn("Call to deprecated function %s." % func.__name__,
category=DeprecationWarning, stacklevel=2)
return func(*args, **kwargs)
return newFunc
@deprecated
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 = rospkg.get_ros_root(env)
if required and not ros_root:
raise rospy.exceptions.ROSException('%s is not set'%rospkg.environment.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 = rosgraph.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 is_shutdown_requested():
"""
is_shutdown_requested is a state that occurs just before
is_shutdown. It is initiated when a shutdown requested is
received and continues until client shutdown handlers have been
called. After client shutdown handlers have been serviced, the
is_shutdown state becomes true.
@return: True if shutdown has been requested (but possibly not yet initiated)
@rtype: bool
"""
return _in_shutdown
def _add_shutdown_hook(h, hooks, pass_reason_argument=True):
"""
shared implementation of add_shutdown_hook and add_preshutdown_hook
"""
if not callable(h):
raise TypeError("shutdown hook [%s] must be a function or callable object: %s"%(h, type(h)))
if _shutdown_flag:
_logger.warn("add_shutdown_hook called after shutdown")
if pass_reason_argument:
h("already shutdown")
else:
h()
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 with zero args
@type h: fn()
"""
_add_shutdown_hook(h, _client_shutdown_hooks, pass_reason_argument=False)
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 as e:
sys.stderr.write("signal_shutdown hook error[%s]\n"%e)
del _shutdown_hooks[:]
threads = _shutdown_threads[:]
for t in threads:
if t.isAlive():
t.join(_TIMEOUT_SHUTDOWN_JOIN)
del _shutdown_threads[:]
try:
rospy.rostime.wallsleep(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
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
return xmlrpcclient.ServerProxy(uri)

97
thirdparty/ros/ros_comm/clients/rospy/src/rospy/exceptions.py vendored Normal file
View File

@@ -0,0 +1,97 @@
# 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 ROSTimeMovedBackwardsException(ROSInterruptException):
"""
Exception if time moved backwards
"""
def __init__(self, time):
self.time = time
"""The amount of time in seconds."""
super(ROSTimeMovedBackwardsException, self).__init__("ROS time moved backwards")
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
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/__init__.py vendored Normal file
View File

111
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/init.py vendored Normal file
View File

@@ -0,0 +1,111 @@
# 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 rosgraph
import rosgraph.roslogging
import rosgraph.xmlrpc
from ..names import _set_caller_id
from ..core import is_shutdown, signal_shutdown, rospyerr
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=0, tcpros_port=0):
"""
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
@param tcpros_port: override the port of the TCP server
@type tcpros_port: int
@return: node server instance
@rtype rosgraph.xmlrpc.XmlRpcNode
@raise ROSInitException: if node has already been started
"""
init_tcpros(tcpros_port)
if not master_uri:
master_uri = rosgraph.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 = rosgraph.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
class RosStreamHandler(rosgraph.roslogging.RosStreamHandler):
def __init__(self, colorize=True):
super(RosStreamHandler, self).__init__(colorize)

548
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/masterslave.py vendored Normal file
View File

@@ -0,0 +1,548 @@
# 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 errno
try:
#py3k
import urllib.parse as urlparse
except ImportError:
import urlparse
from rosgraph.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.__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.__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.__name__)
return -1, "missing required caller_id parameter", error_return_value
elif len(args) != f.__code__.co_argcount:
return -1, "Error: bad call arity", error_return_value
instance = args[0]
caller_id = args[1]
if not isinstance(caller_id, str):
_logger.error("%s: invalid caller_id param type", f.__name__)
return -1, "caller_id must be a string", error_return_value
newArgs = [instance, caller_id] #canonicalized args
try:
for (v, a) in zip(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 as e:
_logger.error("%s: invalid parameter: %s", f.__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.__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 as te: #most likely wrong arg number
_logger.error(traceback.format_exc())
return -1, "Error: invalid arguments: %s"%te, error_return_value
except Exception as e: #internal failure
_logger.error(traceback.format_exc())
return 0, "Internal failure: %s"%e, error_return_value
validated_f.__name__ = f.__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, str):
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("shutdown request: %s"%msg)
else:
print("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: list of topics this node subscribes to.
@rtype: [int, str, [ [topic1, topicType1]...[topicN, topicTypeN]]]
"""
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.)
success = False
interval = 0.5 # seconds
# while the ROS node is not shutdown try to get the topic information
# and retry on connections problems after some wait
# Abort the retry if the we get a Connection Refused since at that point
# we know for sure the URI is invalid
while not success and not is_shutdown():
try:
code, msg, result = \
xmlrpcapi(pub_uri).requestTopic(caller_id, topic, protocols)
success = True
except Exception as e:
if getattr(e, 'errno', None) == errno.ECONNREFUSED:
code = -errno.ECONNREFUSED
msg = str(e)
break
elif not is_shutdown():
_logger.debug("Retrying for %s" % topic)
if interval < 30.0:
# exponential backoff (maximum 32 seconds)
interval = interval * 2
time.sleep(interval)
#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", []

116
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/paramserver.py vendored Normal file
View File

@@ -0,0 +1,116 @@
# 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
#TODO: get rid of this routine entirely. It doesn't work with the current PS representation.
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 = {}
self.notifier = None
## Delete parameter from cache
def delete(self, key):
with self.lock:
del self.d[key]
def set_notifier(self, notifier):
"""
Notifier implements any parameter subscription logic. The
notifier should be a function that takes in a key and value
that represents a parameter update. Notifier is called under
lock and thus must not implement any lengthy computation.
"""
self.notifier = notifier
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.
"""
# TODOXXX: remove this code, as it is wrong. It is remaining
# in place to maintain a unit test until the correct logic is
# implemented. update() needs to check all subscribed keys and
# do a prefix match. The best way to do this is probably to
# pull the paramserver implementation from rosmaster and use
# it here.
if not key in self.d:
raise KeyError(key)
with self.lock:
self.d[key] = value
if self.notifier is not None:
self.notifier(key, value)
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
"""
with self.lock:
self.d[key] = value
def get(self, key):
"""
@param key: parameter key
@type key: str
@return: Current value for parameter
@raise: KeyError
"""
with self.lock:
return self.d[key]
_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

487
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/registration.py vendored Normal file
View File

@@ -0,0 +1,487 @@
# 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"""
import socket
import sys
import logging
import threading
import time
import traceback
try:
import xmlrpc.client as xmlrpcclient
except ImportError:
import xmlrpclib as xmlrpcclient
from rospy.core import is_shutdown, is_shutdown_requested, 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 as e:
logerr("error notifying listener of removal: %s"%traceback.format_exc())
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 as e:
logerr(traceback.format_exc())
def clear(self):
"""
Remove all registration listeners
"""
if not is_shutdown_requested():
with self.lock:
del self.listeners[:]
else:
# when being in shutdown phase the lock might not be lockable
# if a notify_added/removed is currently ongoing
locked = self.lock.acquire(False)
# remove all listeners anyway
del self.listeners[:]
if locked:
self.lock.release()
_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 as 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()
get_topic_manager().check_all()
#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
t = threading.Thread(target=self._connect_topic_thread, args=(topic, uri))
t.setDaemon(True)
t.start()
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 as 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 = xmlrpcclient.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 as se:
(errno, msg) = se.args
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 as 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()

120
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/rosout.py vendored Normal file
View File

@@ -0,0 +1,120 @@
# 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 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
from rosgraph_msgs.msg import Log
_ROSOUT = '/rosout'
_rosout_pub = None
_rospy_to_logging_levels = {
Log.DEBUG: logging.DEBUG,
Log.INFO: logging.INFO,
Log.WARN: logging.WARNING,
Log.ERROR: logging.ERROR,
Log.FATAL: logging.CRITICAL,
}
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, Log, latch=True, queue_size=100)
logger.info("connected to core topic %s"%_ROSOUT)
return True
except Exception as 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, fname, line, func):
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 = Log(level=level, name=str(rospy.names.get_caller_id()), msg=str(msg), topics=topics, file=fname, line=line, function=func)
l.header.stamp = Time.now()
_rosout_pub.publish(l)
finally:
_in_rosout = False
except Exception as 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
_logging_to_rospy_levels = {
logging.DEBUG: Log.DEBUG,
logging.INFO: Log.INFO,
logging.WARNING: Log.WARN,
logging.ERROR: Log.ERROR,
logging.CRITICAL: Log.FATAL,
}
class RosOutHandler(logging.Handler):
def emit(self, record):
_rosout(_logging_to_rospy_levels[record.levelno], record.getMessage(),
record.filename, record.lineno, record.funcName)
## Load loggers for publishing to /rosout
## @param level int: Log level. Loggers >= level will be loaded.
def load_rosout_handlers(level):
logger = logging.getLogger('rosout')
logger.addHandler(RosOutHandler())
if level != None:
logger.setLevel(_rospy_to_logging_levels[level])

93
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/simtime.py vendored Normal file
View File

@@ -0,0 +1,93 @@
# 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 rosgraph
from rosgraph_msgs.msg import Clock
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 = rosgraph.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, Clock, _set_rostime_clock_wrapper, queue_size=1)
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 as e:
logger.error("Unable to initialize %s: %s\n%s", _ROSCLOCK, e, traceback.format_exc())
return False

265
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/statistics.py vendored Normal file
View File

@@ -0,0 +1,265 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2013-2014 Dariush Forouher
# All rights reserved.
#
# Based on code adapted from diagnostics_updater by Blaise Gassend
#
# 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.
from math import sqrt
import logging
import sys
from rosgraph_msgs.msg import TopicStatistics
import rospy
_logger = logging.getLogger('rospy.impl.statistics')
class SubscriberStatisticsLogger():
"""
Class that monitors each subscriber.
this class basically just keeps a collection of ConnectionStatisticsLogger.
"""
@classmethod
def is_enabled(cls):
# disable statistics if node can't talk to parameter server
# which is the case in unit tests
try:
return rospy.get_param("/enable_statistics", False)
except Exception:
return False
def __init__(self, subscriber):
self.subscriber_name = subscriber.name
self.connections = dict()
self.read_parameters()
def read_parameters(self):
"""
Fetch window parameters from parameter server
"""
# Range of window length, in seconds
self.min_elements = rospy.get_param("/statistics_window_min_elements", 10)
self.max_elements = rospy.get_param("/statistics_window_max_elements", 100)
# Range of acceptable messages in window.
# Window size will be adjusted if number of observed is
# outside this range.
self.max_window = rospy.get_param("/statistics_window_max_size", 64)
self.min_window = rospy.get_param("/statistics_window_min_size", 4)
def callback(self, msg, publisher, stat_bytes):
"""
This method is called for every message that has been received.
@param msg: The message received.
@param publisher: The name of the publisher node that sent the msg
@param stat_bytes: A counter, how many bytes have been moved across
this connection since it exists.
This method just looks up the ConnectionStatisticsLogger for the specific connection
between publisher and subscriber and delegates to statistics logging to that
instance.
"""
# /clock is special, as it is subscribed very early
# also exclude /statistics to reduce noise.
if self.subscriber_name == "/clock" or self.subscriber_name == "/statistics":
return
try:
# create ConnectionStatisticsLogger for new connections
logger = self.connections.get(publisher)
if logger is None:
logger = ConnectionStatisticsLogger(self.subscriber_name, rospy.get_name(), publisher)
self.connections[publisher] = logger
# delegate stuff to that instance
logger.callback(self, msg, stat_bytes)
except Exception as e:
rospy.logerr("Unexpected error during statistics measurement: %s", str(e))
def shutdown(self):
for logger in self.connections.values():
logger.shutdown()
self.connections.clear()
class ConnectionStatisticsLogger():
"""
Class that monitors lots of stuff for each connection.
is created whenever a subscriber is created.
is destroyed whenever its parent subscriber is destroyed.
its lifecycle is therefore bound to its parent subscriber.
"""
def __init__(self, topic, subscriber, publisher):
"""
Constructor.
@param topic: Name of the topic
@param subscriber: Name of the subscriber
@param publisher: Name of the publisher
These three should uniquely identify the connection.
"""
self.topic = topic
self.subscriber = subscriber
self.publisher = publisher
self.pub = rospy.Publisher("/statistics", TopicStatistics, queue_size=10)
# reset window
self.last_pub_time = rospy.Time(0)
self.pub_frequency = rospy.Duration(1.0)
# timestamp age
self.age_list_ = []
# period calculations
self.arrival_time_list_ = []
self.last_seq_ = 0
self.dropped_msgs_ = 0
self.window_start = rospy.Time.now()
# temporary variables
self.stat_bytes_last_ = 0
self.stat_bytes_window_ = 0
def sendStatistics(self, subscriber_statistics_logger):
"""
Send out statistics. Aggregate collected stats information.
Currently done blocking. Might be moved to own thread later. But at the moment
any computation done here should be rather quick.
"""
curtime = rospy.Time.now()
msg = TopicStatistics()
msg.topic = self.topic
msg.node_sub = self.subscriber
msg.node_pub = self.publisher
msg.window_start = self.window_start
msg.window_stop = curtime
# Calculate bytes since last message
msg.traffic = self.stat_bytes_window_ - self.stat_bytes_last_
msg.delivered_msgs = len(self.arrival_time_list_)
msg.dropped_msgs = self.dropped_msgs_
# we can only calculate message age if the messages did contain Header fields.
if len(self.age_list_) > 0:
msg.stamp_age_mean = rospy.Duration(sum(self.age_list_, rospy.Duration(0)).to_sec() / len(self.age_list_))
variance = sum((rospy.Duration((msg.stamp_age_mean - value).to_sec() ** 2) for value in self.age_list_), rospy.Duration(0)) / len(self.age_list_)
msg.stamp_age_stddev = rospy.Duration(sqrt(variance.to_sec()))
msg.stamp_age_max = max(self.age_list_)
else:
msg.stamp_age_mean = rospy.Duration(0)
msg.stamp_age_stddev = rospy.Duration(0)
msg.stamp_age_max = rospy.Duration(0)
# computer period/frequency. we need at least two messages within the window to do this.
if len(self.arrival_time_list_) > 1:
periods = [j - i for i, j in zip(self.arrival_time_list_[:-1], self.arrival_time_list_[1:])]
msg.period_mean = rospy.Duration(sum(periods, rospy.Duration(0)).to_sec() / len(periods))
variance = sum((rospy.Duration((msg.period_mean - value).to_sec() ** 2) for value in periods), rospy.Duration(0)) / len(periods)
msg.period_stddev = rospy.Duration(sqrt(variance.to_sec()))
msg.period_max = max(periods)
else:
msg.period_mean = rospy.Duration(0)
msg.period_stddev = rospy.Duration(0)
msg.period_max = rospy.Duration(0)
self.pub.publish(msg)
# adjust window, if message count is not appropriate.
if len(self.arrival_time_list_) > subscriber_statistics_logger.max_elements and self.pub_frequency.to_sec() * 2 <= subscriber_statistics_logger.max_window:
self.pub_frequency *= 2
if len(self.arrival_time_list_) < subscriber_statistics_logger.min_elements and self.pub_frequency.to_sec() / 2 >= subscriber_statistics_logger.min_window:
self.pub_frequency /= 2
# clear collected stats, start new window.
self.age_list_ = []
self.arrival_time_list_ = []
self.dropped_msgs_ = 0
self.window_start = curtime
self.stat_bytes_last_ = self.stat_bytes_window_
def callback(self, subscriber_statistics_logger, msg, stat_bytes):
"""
This method is called for every message, that is received on this
subscriber.
this callback will keep some statistics and publish the results
periodically on a topic. the publishing should probably be done
asynchronically in another thread.
@param msg: The message, that has been received. The message has usually
been already deserialized. However this is not always the case. (AnyMsg)
@param stat_bytes: A counter, how many bytes have been moved across
this connection since it exists.
Any computing-heavy stuff should be done somewhere else, as this
callback has to return before the message is delivered to the user.
"""
arrival_time = rospy.Time.now()
self.arrival_time_list_.append(arrival_time)
self.stat_bytes_window_ = stat_bytes
# rospy has the feature to subscribe a topic with AnyMsg which aren't deserialized.
# Those subscribers won't have a header. But as these subscribers are rather rare
# ("rostopic hz" is the only one I know of), I'm gonna ignore them.
if msg._has_header:
self.age_list_.append(arrival_time - msg.header.stamp)
if self.last_seq_ + 1 != msg.header.seq:
self.dropped_msgs_ = self.dropped_msgs_ + 1
self.last_seq_ = msg.header.seq
# send out statistics with a certain frequency
if self.last_pub_time + self.pub_frequency < arrival_time:
self.last_pub_time = arrival_time
self.sendStatistics(subscriber_statistics_logger)
def shutdown(self):
self.pub.unregister()

62
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/tcpros.py vendored Normal file
View File

@@ -0,0 +1,62 @@
# 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(port=0):
"""
@param tcpros_port: override the port of the TCP server
@type tcpros_port: int
"""
server = init_tcpros_server(port)
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

842
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/tcpros_base.py vendored Normal file
View File

@@ -0,0 +1,842 @@
# 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"""
try:
from cStringIO import StringIO #Python 2.x
python3 = 0
except ImportError:
from io import StringIO, BytesIO #Python 3.x
python3 = 1
import socket
import logging
import threading
import time
import traceback
import select
import rosgraph
import rosgraph.network
from genpy import DeserializationError, Message
from rosgraph.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
_use_tcp_keepalive_lock = threading.Lock()
def _is_use_tcp_keepalive():
global _use_tcp_keepalive
if _use_tcp_keepalive is not None:
return _use_tcp_keepalive
with _use_tcp_keepalive_lock:
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
m = rospy.core.xmlrpcapi(rosgraph.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"""
t = threading.Thread(target=self.run, args=())
t.setDaemon(True)
t.start()
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
except IOError as e:
(errno, msg) = e.args
if errno == 4: #interrupted system call
continue
raise
if self.is_shutdown:
break
try:
#leave threading decisions up to inbound_handler
self.inbound_handler(client_sock, client_addr)
except socket.error as 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 rosgraph.network.get_host_name() instead of address so that it
# obeys ROS_IP/ROS_HOSTNAME behavior
return (rosgraph.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.
"""
if rosgraph.network.use_ipv6():
server_sock = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
server_sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
logdebug('binding to ' + str(rosgraph.network.get_bind_address()) + ' ' + str(self.port))
server_sock.bind((rosgraph.network.get_bind_address(), self.port))
(self.addr, self.port) = server_sock.getsockname()[0:2]
logdebug('bound to ' + str(self.addr) + ' ' + str(self.port))
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(port=0):
"""
starts the TCPROS server socket for inbound connections
@param port: listen on the provided port. If the port number is 0, the port will
be chosen randomly
@type port: int
"""
global _tcpros_server
if _tcpros_server is None:
_tcpros_server = TCPROSServer(port=port)
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, port=0):
"""
Constructur
@param port: port number to bind to (default 0/any)
@type port: int
"""
self.port = port
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):
"""
Starts the TCP socket server if one is not already running
"""
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, self.port)
self.tcp_ros_server.start()
except Exception as 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
if python3 == 0:
#initialize read_ros_handshake_header with BytesIO for Python 3 (instead of bytesarray())
header = read_ros_handshake_header(sock, StringIO(), buff_size)
else:
header = read_ros_handshake_header(sock, BytesIO(), 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.
# We use is_shutdown_requested() because we can get
# into bad connection states during client shutdown
# hooks.
if not rospy.core.is_shutdown_requested():
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 as e:
logwarn(str(e))
if sock is not None:
sock.close()
except Exception as e:
# collect stack trace separately in local log file
if not rospy.core.is_shutdown_requested():
logwarn("Inbound TCP/IP connection failed: %s", e)
rospyerr("Inbound TCP/IP connection failed:\n%s", traceback.format_exc())
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.callerid_pub = 'unknown'
self.dest_address = None # for reconnection
if python3 == 0: # Python 2.x
self.read_buff = StringIO()
self.write_buff = StringIO()
else: # Python 3.x
self.read_buff = BytesIO()
self.write_buff = BytesIO()
#self.write_buff = StringIO()
self.header = header
# #1852 have to hold onto latched messages on subscriber side
self.is_latched = False
self.latch = None
# save the fileno separately so we can garbage collect the
# socket but still unregister will poll objects
self._fileno = 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
# Endpoint Details (IP, Port)
self.local_endpoint = (None, None)
self.remote_endpoint = (None, None)
def get_transport_info(self):
"""
Get detailed connection information.
Similar to getTransportInfo() in 'libros/transport/transport_tcp.cpp'
e.g. TCPROS connection on port 41374 to [127.0.0.1:40623 on socket 6]
"""
return "%s connection on port %s to [%s:%s on socket %s]" % (self.transport_type, self.local_endpoint[1], self.remote_endpoint[0], self.remote_endpoint[1], self._fileno)
def fileno(self):
"""
Get descriptor for select
"""
return self._fileno
def set_endpoint_id(self, endpoint_id):
"""
Set the endpoint_id of this transport.
Allows the endpoint_id to be set before the socket is initialized.
"""
self.endpoint_id = endpoint_id
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
self._fileno = sock.fileno()
self.local_endpoint = self.socket.getsockname()
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
"""
# first make sure that if ROS_HOSTNAME=localhost, we will not attempt
# to connect to anything other than localhost
if ("ROS_HOSTNAME" in os.environ) and (os.environ["ROS_HOSTNAME"] == "localhost"):
if not rosgraph.network.is_local_address(dest_addr):
msg = "attempted to connect to non-local host [%s] from a node launched with ROS_HOSTNAME=localhost" % (dest_addr)
logwarn(msg)
self.close()
raise TransportInitError(msg) # bubble up
# now we can proceed with trying to connect.
try:
self.endpoint_id = endpoint_id
self.dest_address = (dest_addr, dest_port)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
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
logdebug('connecting to ' + str(dest_addr)+ ' ' + str(dest_port))
self.socket.connect((dest_addr, dest_port))
self.write_header()
self.read_header()
self.local_endpoint = self.socket.getsockname()
self.remote_endpoint = (dest_addr, dest_port)
except TransportInitError as 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 as e:
#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()))
# check for error type and reason. On unknown errors the socket will be closed
# to avoid reconnection and error reproduction
if not isinstance(e, socket.error):
# FATAL: no reconnection as error is unknown
self.close()
elif not isinstance(e, socket.timeout) and e.errno not in [100, 101, 102, 103, 110, 112, 113]:
# reconnect in follow cases, otherwise close the socket:
# 1. socket.timeout: on timeouts caused by delays on wireless links
# 2. ENETDOWN (100), ENETUNREACH (101), ENETRESET (102), ECONNABORTED (103):
# while using ROS_HOSTNAME ros binds to a specific interface. Theses errors
# are thrown on interface shutdown e.g. on reconnection in LTE networks
# 3. ETIMEDOUT (110): same like 1. (for completeness)
# 4. EHOSTDOWN (112), EHOSTUNREACH (113): while network and/or DNS-server is not reachable
#
# no reconnection as error is not 1.-4.
self.close()
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.type = header['type']
self.md5sum = header['md5sum']
if 'callerid' in header:
self.callerid_pub = header['callerid']
if header.get('latching', '0') == '1':
self.is_latched = True
def write_header(self):
"""Writes the TCPROS header to the active connection."""
# socket may still be getting spun up, so wait for it to be writable
sock = self.socket
protocol = self.protocol
# race condition on close, better fix is to pass these in,
# functional style, but right now trying to cause minimal
# perturbance to codebase.
if sock is None or protocol is None:
return
fileno = sock.fileno()
ready = None
poller = None
if hasattr(select, 'poll'):
poller = select.poll()
poller.register(fileno, select.POLLOUT)
while not ready:
events = poller.poll()
for _, flag in events:
if flag & select.POLLOUT:
ready = True
else:
while not ready:
try:
_, ready, _ = select.select([], [fileno], [])
except ValueError as e:
logger.error("[%s]: select fileno '%s': %s", self.name, str(fileno), str(e))
raise
logger.debug("[%s]: writing header", self.name)
sock.setblocking(1)
self.stat_bytes += write_ros_handshake_header(sock, protocol.get_header_fields())
if poller:
poller.unregister(fileno)
def read_header(self):
"""
Read TCPROS header from active socket
@raise TransportInitError if header fails to validate
"""
sock = self.socket
if sock is None:
return
sock.setblocking(1)
# TODO: add bytes received to self.stat_bytes
self._validate_header(read_ros_handshake_header(sock, 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 as ioe:
#for now, just document common errno's in code
(errno, msg) = ioe.args
if errno == 32: #broken pipe
logdebug("ERROR: Broken Pipe")
self.close()
raise TransportTerminated(str(errno)+msg)
raise #re-raise
except socket.error as se:
#for now, just document common errno's in code
(errno, msg) = se.args
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)
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:
self.stat_bytes += 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 as e:
rospyerr(traceback.format_exc())
raise TransportException("receive_once[%s]: DeserializationError %s"%(self.name, str(e)))
except TransportTerminated as e:
raise #reraise
except ServiceException as e:
raise
except Exception as e:
rospyerr(traceback.format_exc())
raise TransportException("receive_once[%s]: unexpected error %s"%(self.name, str(e)))
return retval
def _reconnect(self):
# This reconnection logic is very hacky right now. I need to
# rewrite the I/O core so that this is handled more centrally.
if self.dest_address is None:
raise ROSInitException("internal error with reconnection state: address not stored")
interval = 0.5 # seconds
while self.socket is None and not self.done and not rospy.is_shutdown():
try:
# set a timeout so that we can continue polling for
# exit. 30. is a bit high, but I'm concerned about
# embedded platforms. To do this properly, we'd have
# to move to non-blocking routines.
self.connect(self.dest_address[0], self.dest_address[1], self.endpoint_id, timeout=30.)
except TransportInitError:
self.socket = None
if self.socket is None and interval < 30.:
# exponential backoff (maximum 32 seconds)
interval = interval * 2
time.sleep(interval)
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:
while not self.done and not is_shutdown():
try:
if self.socket is not None:
msgs = self.receive_once()
if not self.done and not is_shutdown():
msgs_callback(msgs, self)
else:
self._reconnect()
except TransportException as e:
# set socket to None so we reconnect
try:
if self.socket is not None:
try:
self.socket.shutdown()
except:
pass
finally:
self.socket.close()
except:
pass
self.socket = None
except DeserializationError as e:
#TODO: how should we handle reconnect in this case?
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
rospydebug("receive_loop[%s]: done condition met, exited loop"%self.name)
finally:
if not self.done:
self.close()
def close(self):
"""close i/o and release resources"""
if not self.done:
try:
if self.socket is not None:
try:
self.socket.shutdown(socket.SHUT_RDWR)
except:
pass
finally:
self.socket.close()
finally:
self.socket = self.read_buff = self.write_buff = self.protocol = None
super(TCPROSTransport, self).close()

440
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/tcpros_pubsub.py vendored Normal file
View File

@@ -0,0 +1,440 @@
# 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
import time
try:
from xmlrpc.client import ServerProxy # Python 3.x
except ImportError:
from xmlrpclib import ServerProxy # Python 2.x
from rospy.core import logwarn, logerr, logdebug, rospyerr
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
def robust_connect_subscriber(conn, dest_addr, dest_port, pub_uri, receive_cb, resolved_topic_name):
"""
Keeps trying to create connection for subscriber. Then passes off to receive_loop once connected.
"""
# kwc: this logic is not very elegant. I am waiting to rewrite
# the I/O loop with async i/o to clean this up.
# timeout is really generous. for now just choosing one that is large but not infinite
interval = 0.5
while conn.socket is None and not conn.done and not rospy.is_shutdown():
try:
conn.connect(dest_addr, dest_port, pub_uri, timeout=60.)
except rospy.exceptions.TransportInitError as e:
# if the connection was closed intentionally
# because of an unknown error, stop trying
if conn.protocol is None:
conn.done = True
break
rospyerr("unable to create subscriber transport: %s. Will try again in %ss", e, interval)
if interval < 30.0:
# exponential backoff (maximum 32 seconds)
interval = interval * 2
time.sleep(interval)
# check to see if publisher state has changed
conn.done = not check_if_still_publisher(resolved_topic_name, pub_uri)
if not conn.done:
conn.receive_loop(receive_cb)
def check_if_still_publisher(resolved_topic_name, pub_uri):
try:
s = ServerProxy(pub_uri)
code, msg, val = s.getPublications(rospy.names.get_name())
if code == 1:
return len([t for t in val if t[0] == resolved_topic_name]) > 0
else:
return False
except:
return False
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"%protocol_params[0], 0
id, dest_addr, dest_port = protocol_params
sub = rospy.impl.registration.get_topic_manager().get_subscriber_impl(resolved_name)
#Create connection
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)
conn.set_endpoint_id(pub_uri);
t = threading.Thread(name=resolved_name, target=robust_connect_subscriber, args=(conn, dest_addr, dest_port, pub_uri, sub.receive_callback,resolved_name))
# don't enable this just yet, need to work on this logic
#rospy.core._add_shutdown_thread(t)
# Attach connection to _SubscriberImpl
if sub.add_connection(conn): #pass tcp connection to handler
# since the thread might cause the connection to close
# it should only be started after the connection has been added to the subscriber
# https://github.com/ros/ros_comm/issues/544
t.start()
return 1, "Connected topic[%s]. Transport impl[%s]"%(resolved_name, conn.__class__.__name__), dest_port
else:
# _SubscriberImpl already closed or duplicate subscriber created
conn.close()
return 0, "ERROR: Race condition failure creating topic subscriber [%s]"%(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
"""
if rospy.core.is_shutdown_requested():
return "Node is shutting down"
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 not topic.data_class or topic.closed:
return "Internal error processing topic [%s]"%(resolved_topic_name)
elif md5sum != rospy.names.TOPIC_ANYTYPE and md5sum != topic.data_class._md5sum:
data_class = topic.data_class
actual_type = 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, 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.remote_endpoint = client_addr
transport.write_header()
topic.add_connection(transport)
class QueuedConnection(object):
"""
It wraps a Transport instance and behaves like one
but it queues the data written to it and relays them
asynchronously to the wrapped instance.
"""
def __init__(self, connection, queue_size):
"""
ctor.
@param connection: the wrapped transport instance
@type connection: Transport
@param queue_size: the maximum size of the queue, zero means infinite
@type queue_size: int
"""
super(QueuedConnection, self).__init__()
self._connection = connection
self._queue_size = queue_size
self._lock = threading.Lock()
self._cond_data_available = threading.Condition(self._lock)
self._connection.set_cleanup_callback(self._closed_connection_callback)
self._queue = []
self._error = None
self._thread = threading.Thread(target=self._run)
self._thread.start()
def _closed_connection_callback(self, connection):
with self._lock:
self._cond_data_available.notify()
def __getattr__(self, name):
if name.startswith('__'):
raise AttributeError(name)
return getattr(self._connection, name)
def write_data(self, data):
with self._lock:
# if there was previously an error within the dispatch thread raise it
if self._error:
error = self._error
self._error = None
raise error
# pop oldest data if queue limit is reached
if self._queue_size > 0 and len(self._queue) == self._queue_size:
del self._queue[0]
self._queue.append(data)
self._cond_data_available.notify()
# effectively yields the rest of the thread quantum
time.sleep(0)
return True
def _run(self):
while not self._connection.done:
queue = []
with self._lock:
# wait for available data
while not self._queue and not self._connection.done:
self._cond_data_available.wait()
# take all data from queue for processing outside of the lock
if self._queue:
queue = self._queue
self._queue = []
# relay all data
for data in queue:
try:
self._connection.write_data(data)
except Exception as e:
with self._lock:
self._error = e

714
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/tcpros_service.py vendored Normal file
View File

@@ -0,0 +1,714 @@
# 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.
"""Internal use: Service-specific extensions for TCPROS support"""
import io
import socket
import struct
import sys
import logging
import time
import traceback
import genpy
import rosgraph
import rosgraph.names
import rosgraph.network
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
import threading
if sys.hexversion > 0x03000000: #Python3
def isstring(s):
return isinstance(s, str) #Python 3.x
else:
def isstring(s):
return isinstance(s, basestring) #Python 2.x
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
"""
master = rosgraph.Master(rospy.names.get_caller_id())
def contact_service(resolved_name, timeout=10.0):
try:
uri = master.lookupService(resolved_name)
except rosgraph.MasterException:
return False
addr = rospy.core.parse_rosrpc_uri(uri)
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_STREAM)
else:
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)
logdebug('connecting to ' + str(addr))
s.connect(addr)
h = { 'probe' : '1', 'md5sum' : '*',
'callerid' : rospy.core.get_caller_id(),
'service': resolved_name }
rosgraph.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)
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
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, will keep trying"%(resolved_name))
time.sleep(0.3)
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
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, will keep trying"%(resolved_name))
time.sleep(0.3)
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, genpy.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 as 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 as 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 as 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
t = threading.Thread(target=service.handle, args=(transport, header))
t.setDaemon(True)
t.start()
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.items():
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, genpy.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:
master = rosgraph.Master(rospy.names.get_caller_id())
self.uri = master.lookupService(self.resolved_name)
except socket.error:
raise ServiceException("unable to contact master")
except rosgraph.MasterError as e:
logger.error("[%s]: lookup service failed with message [%s]", self.resolved_name, str(e))
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 as 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 as 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 as 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, error_handler=None):
super(ServiceImpl, self).__init__(name, service_class)
if not name or not isstring(name):
raise ValueError("service name is not a non-empty string")
# #2202
if not rosgraph.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
if error_handler is not None:
self.error_handler = error_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 error_handler(self, e, exc_type, exc_value, tb):
logerr("Error processing request: %s\n%s" % (e, traceback.format_exception(exc_type, exc_value, tb)))
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 as 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
"""
if sys.hexversion > 0x03000000: #Python3
err_msg = bytes(err_msg, 'utf-8')
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: genpy.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 as e:
rospy.core.rospydebug("handler raised ServiceException: %s"%(e))
self._write_service_error(transport, "service cannot process request: %s"%e)
except Exception as e:
(exc_type, exc_value, tb) = sys.exc_info()
self.error_handler(e, exc_type, exc_value, tb)
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 as 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, error_handler=None):
"""
ctor.
@param name: service name, ``str``
@param service_class: Service definition 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
@param error_handler: callback function for handling errors
raised in the service code.
@type error_handler: fn(exception, exception_type, exception_value, traceback)->None
"""
super(Service, self).__init__(name, service_class, handler, buff_size,
error_handler)
#TODO: make service manager configurable
get_service_manager().register(self.resolved_name, self)

211
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/transport.py vendored Normal file
View File

@@ -0,0 +1,211 @@
# 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
# Endpoint Details (IP, Port)
self.local_endpoint = (0, 0)
self.remote_endpoint = (0, 0)
def fileno(self):
"""
Get a file descriptor for select() if available
"""
return None
## 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")
## Implements the getTransportInfo() from roscpp
## Similar to getTransportInfo() in 'libros/transport/transport_tcp.cpp'
def get_transport_info(self):
raise NotImplementedError
## 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
self.local_endpoint = transport.local_endpoint
self.remote_endpoint = transport.remote_endpoint
## @param self
def get_transport_info(self):
return "Closed %s connection on port %s to [%s:%s]" % (self.transport_type, self.local_endpoint[1], self.remote_endpoint[0], self.remote_endpoint[1])
## 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")

306
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/udpros.py vendored Normal file
View File

@@ -0,0 +1,306 @@
# 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$
from __future__ import print_function
"""
UDPROS connection protocol.
"""
## UDPROS connection protocol.
# http://ros.org/wiki/ROS/UDPROS
#
import rosgraph.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
if rosgraph.network.use_ipv6():
s = socket.socket(socket.AF_INET6, socket.SOCK_DGRAM)
else:
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.bind((rosgraph.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
thirdparty/ros/ros_comm/clients/rospy/src/rospy/impl/validators.py vendored Normal file
View File

@@ -0,0 +1,51 @@
# 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

147
thirdparty/ros/ros_comm/clients/rospy/src/rospy/logger_level_service_caller.py vendored Normal file
View File

@@ -0,0 +1,147 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2015, Chris Mansley, Open Source Robotics Foundation, 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.
import rosgraph
import rosnode
import rospy
import rosservice
class ROSConsoleException(Exception):
"""Base exception class of rosconsole-related errors."""
pass
class LoggerLevelServiceCaller(object):
"""
Handles service calls for getting lists of nodes and loggers.
Also handles sending requests to change logger levels.
"""
def __init__(self):
pass
def get_levels(self):
# Declare level names lower-case, because that's how they are returned
# from the service call.
return ['debug', 'info', 'warn', 'error', 'fatal']
def get_loggers(self, node):
self._refresh_loggers(node)
return self._current_loggers
def get_node_names(self):
"""
Get a list of available services via a ros service call.
:returns: a list of all nodes that provide the set_logger_level service, ''list(str)''
"""
set_logger_level_nodes = []
nodes = rosnode.get_node_names()
for name in sorted(nodes):
for service in rosservice.get_service_list(name):
if service == name + '/set_logger_level':
set_logger_level_nodes.append(name)
return set_logger_level_nodes
def _refresh_loggers(self, node):
"""
Store a list of loggers available for passed in node.
:param node: name of the node to query, ''str''
:raises: :exc:`ROSTopicException` If topic type cannot be determined or loaded
"""
self._current_loggers = []
self._current_levels = {}
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.ns_join(
rosgraph.names.ns_join(rosgraph.names.get_ros_namespace(), node),
'get_loggers')
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.resolve_name(
servicename, rosgraph.names.get_ros_namespace())
try:
service = rosservice.get_service_class_by_name(servicename)
except rosservice.ROSServiceException as e:
raise ROSConsoleException(
"node '%s' doesn't exist or doesn't support query: %s" % (node, e))
request = service._request_class()
proxy = rospy.ServiceProxy(str(servicename), service)
try:
response = proxy(request)
except rospy.ServiceException as e:
raise ROSConsoleException("node '%s' logger request failed: %s" % (node, e))
if response._slot_types[0] == 'roscpp/Logger[]':
for logger in getattr(response, response.__slots__[0]):
self._current_loggers.append(logger.name)
self._current_levels[logger.name] = logger.level
else:
raise ROSConsoleException(repr(response))
def send_logger_change_message(self, node, logger, level):
"""
Send a logger level change request to 'node'.
:param node: name of the node to chaange, ''str''
:param logger: name of the logger to change, ''str''
:param level: name of the level to change, ''str''
:returns: True if the response is valid, ''bool''
:returns: False if the request raises an exception or would not change the state, ''bool''
"""
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.ns_join(
rosgraph.names.ns_join(rosgraph.names.get_ros_namespace(), node),
'set_logger_level')
# Construct the service name, taking into account our namespace
servicename = rosgraph.names.resolve_name(
servicename, rosgraph.names.get_ros_namespace())
if self._current_levels[logger] == level:
return False
service = rosservice.get_service_class_by_name(servicename)
request = service._request_class()
request.logger = logger
request.level = level
proxy = rospy.ServiceProxy(str(servicename), service)
try:
proxy(request)
self._current_levels[logger] = level.upper()
except rospy.ServiceException as e:
raise ROSConsoleException("node '%s' logger request failed: %s" % (node, e))
return True

246
thirdparty/ros/ros_comm/clients/rospy/src/rospy/msg.py vendored Normal file
View File

@@ -0,0 +1,246 @@
# 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 genpy
import rospy.exceptions
import rospy.names
class AnyMsg(genpy.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.ROSException("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, genpy.Message):
if len(data_class._slot_types) == 0:
raise TypeError("expected [] but got [%s]"%arg._type)
elif arg._type != data_class._slot_types[0]:
raise TypeError("expected [%s] but got [%s]"%(data_class._slot_types[0], arg._type))
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 as 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 genpy.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 as e:
logging.getLogger('rospy.msg').error("cannot deserialize message: EXCEPTION %s", traceback.format_exc())
raise genpy.DeserializationError("cannot deserialize: %s"%str(e))

209
thirdparty/ros/ros_comm/clients/rospy/src/rospy/msproxy.py vendored Normal file
View File

@@ -0,0 +1,209 @@
# 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.
"""
from threading import Lock
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
All methods are thread-safe.
"""
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)
self._lock = Lock()
def __getattr__(self, key): #forward api calls to target
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])
with self._lock:
f = getattr(self.target, key)
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
with self._lock:
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
with self._lock:
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
"""
with self._lock:
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]
with self._lock:
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)
with self._lock:
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
"""
with self._lock:
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
"""
with self._lock:
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)

338
thirdparty/ros/ros_comm/clients/rospy/src/rospy/names.py vendored Normal file
View File

@@ -0,0 +1,338 @@
# 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 rosgraph.names import namespace, get_ros_namespace, ns_join, make_global_ns, load_mappings, \
SEP, GLOBALNS, REMAP, ANYTYPE, \
is_global, is_private
import rosgraph.names
from rospy.exceptions import ROSException, ROSInitException
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
import struct
if sys.hexversion > 0x03000000: #Python3
def isstring(s):
return isinstance(s, str) #Python 3.x
else:
def isstring(s):
return isinstance(s, basestring) #Python 2.x
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.items():
# resolve both parts of the mappings. use the rosgraph.names
# version of resolve_name to avoid circular mapping.
if m.startswith('__'): # __name, __log, etc...
_resolved_mappings[m] = v
else:
_resolved_mappings[rosgraph.names.resolve_name(m, node_name)] = rosgraph.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 rosgraph.names.resolve_name to avoid dependencies on nodename/remappings
fake_caller_id = ns_join(get_namespace(), 'node')
fake_resolved = rosgraph.names.resolve_name(name, fake_caller_id)
for m, v in _mappings.items():
if rosgraph.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 rosgraph.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 rosgraph.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 = str(name) # enforce string conversion else struct.pack might raise UnicodeDecodeError (see #3998)
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 rosgraph.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 isstring(param_value):
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 rosgraph.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 isstring(param_value):
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 isstring(param_value):
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)

99
thirdparty/ros/ros_comm/clients/rospy/src/rospy/numpy_msg.py vendored Normal file
View File

@@ -0,0 +1,99 @@
# 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), queue_size=10)
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)
_numpy_msg_types = {}
## 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):
if msg_type in _numpy_msg_types:
return _numpy_msg_types[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('/', '__')
numpy_type = type(msg_type_name,(msg_type,),classdict)
_numpy_msg_types[msg_type] = numpy_type
return numpy_type

185
thirdparty/ros/ros_comm/clients/rospy/src/rospy/rosconsole.py vendored Normal file
View File

@@ -0,0 +1,185 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2015, Chris Mansley, Open Source Robotics Foundation, 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.
from __future__ import print_function
import os
import socket
import sys
import rosgraph
import rospy
from .logger_level_service_caller import LoggerLevelServiceCaller
from .logger_level_service_caller import ROSConsoleException
NAME = 'rosconsole'
def error(status, msg):
print("%s: error: %s" % (NAME, msg), file=sys.stderr)
sys.exit(status)
def _get_cmd_list_optparse():
from optparse import OptionParser
usage = "usage: %prog list <node>"
parser = OptionParser(usage=usage, prog=NAME)
return parser
def _rosconsole_cmd_list(argv):
args = argv[2:]
parser = _get_cmd_list_optparse()
(options, args) = parser.parse_args(args)
if not args:
parser.error("you must specify a node to list loggers")
elif len(args) > 1:
parser.error("you may only specify one node to list")
logger_level = LoggerLevelServiceCaller()
loggers = logger_level.get_loggers(args[0])
output = '\n'.join(loggers)
print(output)
def _get_cmd_set_optparse():
from optparse import OptionParser
usage = "usage: %prog set <node> <logger> <level>"
levels = ', '.join(LoggerLevelServiceCaller().get_levels())
usage += "\n\n <level> must be one of [" + levels + "]"
parser = OptionParser(usage=usage, prog=NAME)
return parser
def _rosconsole_cmd_set(argv):
args = argv[2:]
parser = _get_cmd_set_optparse()
(options, args) = parser.parse_args(args)
if len(args) < 3:
parser.error("you must specify a node, a logger and a level")
logger_level = LoggerLevelServiceCaller()
logger_level.get_loggers(args[0])
if args[1] not in logger_level._current_levels:
error(2, "node " + args[0] + " does not contain logger " + args[1])
level = args[2].lower()
if level not in logger_level.get_levels():
parser.error("invalid level")
logger_level.send_logger_change_message(args[0], args[1], args[2])
def _get_cmd_get_optparse():
from optparse import OptionParser
usage = "usage: %prog get <node> <logger>"
parser = OptionParser(usage=usage, prog=NAME)
return parser
def _rosconsole_cmd_get(argv):
args = argv[2:]
parser = _get_cmd_get_optparse()
(options, args) = parser.parse_args(args)
if len(args) < 2:
parser.error("you must specify a node and a logger")
logger_level = LoggerLevelServiceCaller()
logger_level.get_loggers(args[0])
if args[1] not in logger_level._current_levels:
error(2, "node " + args[0] + " does not contain logger " + args[1])
print(logger_level._current_levels[args[1]])
def _fullusage():
print("""rosconsole is a command-line tool for configuring the logger level of ROS nodes.
Commands:
\trosconsole get\tdisplay level for a logger
\trosconsole list\tlist loggers for a node
\trosconsole set\tset level for a logger
Type rosconsole <command> -h for more detailed usage, e.g. 'rosconsole list -h'
""")
sys.exit(getattr(os, 'EX_USAGE', 1))
def main(argv=None):
if argv is None:
argv = sys.argv
# Initialize ourselves as a node, to ensure handling of namespace and
# remapping arguments
rospy.init_node('rosconsole', anonymous=True)
argv = rospy.myargv(argv)
# process argv
if len(argv) == 1:
_fullusage()
try:
command = argv[1]
if command == 'get':
_rosconsole_cmd_get(argv)
elif command == 'list':
_rosconsole_cmd_list(argv)
elif command == 'set':
_rosconsole_cmd_set(argv)
else:
_fullusage()
except socket.error as e:
error(1,
"Network communication failed; most likely failed to communicate with master: %s" % e)
except rosgraph.MasterException as e:
# mainly for invalid master URI/rosgraph.masterapi
error(1, str(e))
except ROSConsoleException as e:
error(1, str(e))
except KeyboardInterrupt:
pass
except rospy.ROSInterruptException:
pass

277
thirdparty/ros/ros_comm/clients/rospy/src/rospy/rostime.py vendored Normal file
View File

@@ -0,0 +1,277 @@
# 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 sys
import threading
import time
import traceback
import rospy.exceptions
import genpy
## /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 genpy to provide abstraction layer
class Duration(genpy.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
"""
super(Duration, self).__init__(secs, nsecs)
def __repr__(self):
return 'rospy.Duration[%d]' % self.to_nsec()
class Time(genpy.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
"""
super(Time, self).__init__(secs, nsecs)
def __repr__(self):
return 'rospy.Time[%d]' % self.to_nsec()
@staticmethod
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}
"""
return get_rostime()
@classmethod
def from_seconds(cls, 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 cls.from_sec(float_secs)
def _set_rostime(t):
"""Callback to update ROS time from a ROS Topic"""
if isinstance(t, genpy.Time):
t = Time(t.secs, t.nsecs)
elif not isinstance(t, Time):
raise ValueError("must be Time instance: %s"%t.__class__)
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}
"""
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)
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()
def wallsleep(duration):
"""
Internal use.
Windows interrupts time.sleep with an IOError exception
when a signal is caught. Even when the signal is handled
by a callback, it will then proceed to throw IOError when
the handling has completed.
Refer to https://code.ros.org/trac/ros/ticket/3421.
So we create a platform dependant wrapper to handle this
here.
"""
if sys.platform in ['win32']: # cygwin seems to be ok
try:
time.sleep(duration)
except IOError:
pass
else:
time.sleep(duration)

143
thirdparty/ros/ros_comm/clients/rospy/src/rospy/service.py vendored Normal file
View File

@@ -0,0 +1,143 @@
# 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."""
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')
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.items():
ret_val.append((name, service.uri))
services = list(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())

240
thirdparty/ros/ros_comm/clients/rospy/src/rospy/timer.py vendored Normal file
View File

@@ -0,0 +1,240 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2010, 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: __init__.py 12069 2010-11-09 20:31:55Z kwc $
import threading
import time
# for Time, Duration
import genpy
import rospy.core
import rospy.rostime
# author: tfield (Timers)
# author: kwc (Rate, sleep)
class Rate(object):
"""
Convenience class for sleeping in a loop at a specified rate
"""
def __init__(self, hz, reset=False):
"""
Constructor.
@param hz: hz rate to determine sleeping
@type hz: float
@param reset: if True, timer is reset when rostime moved backward. [default: False]
@type reset: bool
"""
# #1403
self.last_time = rospy.rostime.get_rostime()
self.sleep_dur = rospy.rostime.Duration(0, int(1e9/hz))
self._reset = reset
def _remaining(self, curr_time):
"""
Calculate the time remaining for rate to sleep.
@param curr_time: current time
@type curr_time: L{Time}
@return: time remaining
@rtype: L{Time}
"""
# detect time jumping backwards
if self.last_time > curr_time:
self.last_time = curr_time
# calculate remaining time
elapsed = curr_time - self.last_time
return self.sleep_dur - elapsed
def remaining(self):
"""
Return the time remaining for rate to sleep.
@return: time remaining
@rtype: L{Time}
"""
curr_time = rospy.rostime.get_rostime()
return self._remaining(curr_time)
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 shutdown occurs before
sleep completes
@raise ROSTimeMovedBackwardsException: if ROS time is set
backwards
"""
curr_time = rospy.rostime.get_rostime()
try:
sleep(self._remaining(curr_time))
except rospy.exceptions.ROSTimeMovedBackwardsException:
if not self._reset:
raise
self.last_time = rospy.rostime.get_rostime()
return
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
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 shutdown occurs before sleep
completes
@raise ROSTimeMovedBackwardsException: if ROS time is set
backwards
"""
if rospy.rostime.is_wallclock():
if isinstance(duration, genpy.Duration):
duration = duration.to_sec()
if duration < 0:
return
else:
rospy.rostime.wallsleep(duration)
else:
initial_rostime = rospy.rostime.get_rostime()
if not isinstance(duration, genpy.Duration):
duration = genpy.Duration.from_sec(duration)
rostime_cond = rospy.rostime.get_rostime_cond()
# #3123
if initial_rostime == genpy.Time(0):
# break loop if time is initialized or node is shutdown
while initial_rostime == genpy.Time(0) and \
not rospy.core.is_shutdown():
with rostime_cond:
rostime_cond.wait(0.3)
initial_rostime = rospy.rostime.get_rostime()
sleep_t = initial_rostime + duration
# 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():
with rostime_cond:
rostime_cond.wait(0.5)
if rospy.rostime.get_rostime() < initial_rostime:
time_jump = (initial_rostime - rospy.rostime.get_rostime()).to_sec()
rospy.core.logerr("ROS time moved backwards: %ss", time_jump)
raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump)
if rospy.core.is_shutdown():
raise rospy.exceptions.ROSInterruptException("ROS shutdown request")
class TimerEvent(object):
"""
Constructor.
@param last_expected: in a perfect world, this is when the previous callback should have happened
@type last_expected: rospy.Time
@param last_real: when the callback actually happened
@type last_real: rospy.Time
@param current_expected: in a perfect world, this is when the current callback should have been called
@type current_expected: rospy.Time
@param last_duration: contains the duration of the last callback (end time minus start time) in seconds.
Note that this is always in wall-clock time.
@type last_duration: float
"""
def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):
self.last_expected = last_expected
self.last_real = last_real
self.current_expected = current_expected
self.current_real = current_real
self.last_duration = last_duration
class Timer(threading.Thread):
"""
Convenience class for calling a callback at a specified rate
"""
def __init__(self, period, callback, oneshot=False, reset=False):
"""
Constructor.
@param period: desired period between callbacks
@type period: rospy.Duration
@param callback: callback to be called
@type callback: function taking rospy.TimerEvent
@param oneshot: if True, fire only once, otherwise fire continuously until shutdown is called [default: False]
@type oneshot: bool
@param reset: if True, timer is reset when rostime moved backward. [default: False]
@type reset: bool
"""
super(Timer, self).__init__()
self._period = period
self._callback = callback
self._oneshot = oneshot
self._reset = reset
self._shutdown = False
self.setDaemon(True)
self.start()
def shutdown(self):
"""
Stop firing callbacks.
"""
self._shutdown = True
def run(self):
r = Rate(1.0 / self._period.to_sec(), reset=self._reset)
current_expected = rospy.rostime.get_rostime() + self._period
last_expected, last_real, last_duration = None, None, None
while not rospy.core.is_shutdown() and not self._shutdown:
try:
r.sleep()
except rospy.exceptions.ROSInterruptException as e:
if rospy.core.is_shutdown():
break
raise
if self._shutdown:
break
current_real = rospy.rostime.get_rostime()
start = time.time()
self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))
if self._oneshot:
break
last_duration = time.time() - start
last_expected, last_real = current_expected, current_real
current_expected += self._period

1365
thirdparty/ros/ros_comm/clients/rospy/src/rospy/topics.py vendored Normal file
View File

File diff suppressed because it is too large Load Diff

60
thirdparty/ros/ros_comm/clients/rospy/test_nodes/listener.py vendored Executable file
View File

@@ -0,0 +1,60 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
## Simple talker demo that listens to std_msgs/Strings published
## to the 'chatter' topic
PKG = 'rosmaster' # this package name
import rospy
from std_msgs.msg import String
def callback(data):
rospy.loginfo(rospy.get_caller_id()+"I heard %s",data.data)
def listener():
# in ROS, nodes are unique named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'talker' node so that multiple talkers can
# run simultaenously.
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("chatter", String, callback)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
listener()

53
thirdparty/ros/ros_comm/clients/rospy/test_nodes/talker.py vendored Executable file
View File

@@ -0,0 +1,53 @@
#!/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.
## Simple talker demo that published std_msgs/Strings messages
## to the 'chatter' topic
import rospy
from std_msgs.msg import String
def talker():
rospy.init_node('talker', anonymous=True)
pub = rospy.Publisher('chatter', String, queue_size=10)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
str = "hello world %s"%rospy.get_time()
rospy.loginfo(str)
pub.publish(str)
r.sleep()
if __name__ == '__main__':
try:
talker()
except rospy.ROSInterruptException: pass