v01
This commit is contained in:
Ivan
257
thirdparty/ros/ros_comm/clients/roscpp/CHANGELOG.rst
vendored
Normal file
257
thirdparty/ros/ros_comm/clients/roscpp/CHANGELOG.rst
vendored
Normal file
@@ -0,0 +1,257 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package roscpp
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
1.12.14 (2018-08-23)
|
||||
--------------------
|
||||
* add hasStarted() to Timer API (`#1464 <https://github.com/ros/ros_comm/issues/1464>`_)
|
||||
* force a rebuild of the pollset on flag changes (`#1393 <https://github.com/ros/ros_comm/issues/1393>`_)
|
||||
* fix integer overflow for oneshot timers (`#1382 <https://github.com/ros/ros_comm/issues/1382>`_)
|
||||
* convert the period standard deviation in StatisticsLogger to Duration at the end (`#1361 <https://github.com/ros/ros_comm/issues/1361>`_)
|
||||
* replace DCL pattern with static variable (`#1365 <https://github.com/ros/ros_comm/issues/1365>`_)
|
||||
|
||||
1.12.13 (2018-02-21)
|
||||
--------------------
|
||||
* avoid recreating poll set (`#1281 <https://github.com/ros/ros_comm/pull/1281>`_)
|
||||
* switch to using epoll (`#1217 <https://github.com/ros/ros_comm/pull/1217>`_)
|
||||
* monotonic clock for callback queue timeouts (`#1250 <https://github.com/ros/ros_comm/pull/1250>`_)
|
||||
* fix IPv6 initialization order (`#1262 <https://github.com/ros/ros_comm/issues/1262>`_)
|
||||
* changed error message for single threaded spinner (`#1164 <https://github.com/ros/ros_comm/pull/1164>`_)
|
||||
|
||||
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)
|
||||
-------------------
|
||||
* check if socket options are available before using them (`#1172 <https://github.com/ros/ros_comm/issues/1172>`_)
|
||||
* only use CLOCK_MONOTONIC if not on OS X (`#1142 <https://github.com/ros/ros_comm/issues/1142>`_)
|
||||
* xmlrpc_manager: use SteadyTime for timeout (`#1134 <https://github.com/ros/ros_comm/issues/1134>`_)
|
||||
* ignore headers with zero stamp in statistics (`#1127 <https://github.com/ros/ros_comm/issues/1127>`_)
|
||||
* add SteadyTimer, used in TimerManager (`#1014 <https://github.com/ros/ros_comm/issues/1014>`_)
|
||||
* include missing header for writev() (`#1105 <https://github.com/ros/ros_comm/pull/1105>`_)
|
||||
* add missing mutex lock for publisher links (`#1090 <https://github.com/ros/ros_comm/pull/1090>`_)
|
||||
* fix race condition that lead to miss first message (`#1058 <https://github.com/ros/ros_comm/issues/1058>`_)
|
||||
* fix bug in transport_tcp on Windows (`#1050 <https://github.com/ros/ros_comm/issues/1050>`_)
|
||||
* add subscriber to connection log messages (`#1023 <https://github.com/ros/ros_comm/issues/1023>`_)
|
||||
* avoid deleting XmlRpcClient while being used in another thread (`#1013 <https://github.com/ros/ros_comm/issues/1013>`_)
|
||||
|
||||
1.12.7 (2017-02-17)
|
||||
-------------------
|
||||
* move connection specific log message to new name roscpp_internal.connections (`#980 <https://github.com/ros/ros_comm/pull/980>`_)
|
||||
* move headers to include/xmlrpcpp (`#962 <https://github.com/ros/ros_comm/issues/962>`_)
|
||||
* fix UDP block number when EAGAIN or EWOULDBLOCK (`#957 <https://github.com/ros/ros_comm/issues/957>`_)
|
||||
* fix return code of master execute function (`#938 <https://github.com/ros/ros_comm/pull/938>`_)
|
||||
* change WallTimerEvent from class to struct (`#924 <https://github.com/ros/ros_comm/pull/924>`_)
|
||||
|
||||
1.12.6 (2016-10-26)
|
||||
-------------------
|
||||
|
||||
1.12.5 (2016-09-30)
|
||||
-------------------
|
||||
|
||||
1.12.4 (2016-09-19)
|
||||
-------------------
|
||||
|
||||
1.12.3 (2016-09-17)
|
||||
-------------------
|
||||
* fix multi-threaded spinning (`#867 <https://github.com/ros/ros_comm/pull/867>`_)
|
||||
* fix static destruction order (`#871 <https://github.com/ros/ros_comm/pull/871>`_)
|
||||
* throw exception on ros::init with empty node name (`#894 <https://github.com/ros/ros_comm/pull/894>`_)
|
||||
* improve debug message when queue is full (`#818 <https://github.com/ros/ros_comm/issues/818>`_)
|
||||
|
||||
1.12.2 (2016-06-03)
|
||||
-------------------
|
||||
* improve stacktrace for exceptions thrown in callbacks (`#811 <https://github.com/ros/ros_comm/pull/811>`_)
|
||||
* fix segfault if creating outgoing UDP transport fails (`#807 <https://github.com/ros/ros_comm/pull/807>`_)
|
||||
|
||||
1.12.1 (2016-04-18)
|
||||
-------------------
|
||||
* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
|
||||
|
||||
1.12.0 (2016-03-18)
|
||||
-------------------
|
||||
* improve TopicManager::instance (`#770 <https://github.com/ros/ros_comm/issues/770>`_)
|
||||
* change return value of param() to bool (`#753 <https://github.com/ros/ros_comm/issues/753>`_)
|
||||
|
||||
1.11.18 (2016-03-17)
|
||||
--------------------
|
||||
* fix CMake warning about non-existing targets
|
||||
|
||||
1.11.17 (2016-03-11)
|
||||
--------------------
|
||||
* fix order of argument in SubscriberLink interface to match actual implemenation (`#701 <https://github.com/ros/ros_comm/issues/701>`_)
|
||||
* add method for getting all the parameters from the parameter server as implemented in the rospy client (`#739 <https://github.com/ros/ros_comm/issues/739>`_)
|
||||
* use boost::make_shared instead of new for constructing boost::shared_ptr (`#740 <https://github.com/ros/ros_comm/issues/740>`_)
|
||||
* fix max elements param for statistics window (`#750 <https://github.com/ros/ros_comm/issues/750>`_)
|
||||
* improve NodeHandle constructor documentation (`#692 <https://github.com/ros/ros_comm/issues/692>`_)
|
||||
|
||||
1.11.16 (2015-11-09)
|
||||
--------------------
|
||||
* add getROSArg function (`#694 <https://github.com/ros/ros_comm/pull/694>`_)
|
||||
|
||||
1.11.15 (2015-10-13)
|
||||
--------------------
|
||||
* fix crash in onRetryTimer() callback (`#577 <https://github.com/ros/ros_comm/issues/577>`_)
|
||||
|
||||
1.11.14 (2015-09-19)
|
||||
--------------------
|
||||
* add optional reset argument to Timer::setPeriod() (`#590 <https://github.com/ros/ros_comm/issues/590>`_)
|
||||
* add getParam() and getParamCached() for float (`#621 <https://github.com/ros/ros_comm/issues/621>`_, `#623 <https://github.com/ros/ros_comm/issues/623>`_)
|
||||
* use explicit bool cast to compile with C++11 (`#632 <https://github.com/ros/ros_comm/pull/632>`_)
|
||||
|
||||
1.11.13 (2015-04-28)
|
||||
--------------------
|
||||
|
||||
1.11.12 (2015-04-27)
|
||||
--------------------
|
||||
|
||||
1.11.11 (2015-04-16)
|
||||
--------------------
|
||||
* fix memory leak in transport constructor (`#570 <https://github.com/ros/ros_comm/pull/570>`_)
|
||||
* fix computation of stddev in statistics (`#556 <https://github.com/ros/ros_comm/pull/556>`_)
|
||||
* fix empty connection header topic (`#543 <https://github.com/ros/ros_comm/issues/543>`_)
|
||||
* alternative API to get parameter values (`#592 <https://github.com/ros/ros_comm/pull/592>`_)
|
||||
* add getCached() for float parameters (`#584 <https://github.com/ros/ros_comm/pull/584>`_)
|
||||
|
||||
1.11.10 (2014-12-22)
|
||||
--------------------
|
||||
* fix various defects reported by coverity
|
||||
* fix comment (`#529 <https://github.com/ros/ros_comm/issues/529>`_)
|
||||
* improve Android support (`#518 <https://github.com/ros/ros_comm/pull/518>`_)
|
||||
|
||||
1.11.9 (2014-08-18)
|
||||
-------------------
|
||||
* add accessor to expose whether service is persistent (`#489 <https://github.com/ros/ros_comm/issues/489>`_)
|
||||
* populate delivered_msgs field of TopicStatistics message (`#486 <https://github.com/ros/ros_comm/issues/486>`_)
|
||||
|
||||
1.11.8 (2014-08-04)
|
||||
-------------------
|
||||
* fix C++11 compatibility issue (`#483 <https://github.com/ros/ros_comm/issues/483>`_)
|
||||
|
||||
1.11.7 (2014-07-18)
|
||||
-------------------
|
||||
* fix segfault due to accessing a NULL pointer for some network interfaces (`#465 <https://github.com/ros/ros_comm/issues/465>`_) (regression from 1.11.6)
|
||||
|
||||
1.11.6 (2014-07-10)
|
||||
-------------------
|
||||
* 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)
|
||||
-------------------
|
||||
* improve handling dropped connections (`#434 <https://github.com/ros/ros_comm/issues/434>`_)
|
||||
* add header needed for Android (`#441 <https://github.com/ros/ros_comm/issues/441>`_)
|
||||
* fix typo for parameter used for statistics (`#448 <https://github.com/ros/ros_comm/issues/448>`_)
|
||||
|
||||
1.11.4 (2014-06-16)
|
||||
-------------------
|
||||
|
||||
1.11.3 (2014-05-21)
|
||||
-------------------
|
||||
|
||||
1.11.2 (2014-05-08)
|
||||
-------------------
|
||||
|
||||
1.11.1 (2014-05-07)
|
||||
-------------------
|
||||
* update API to use boost::signals2 (`#267 <https://github.com/ros/ros_comm/issues/267>`_)
|
||||
* only update param cache when being subscribed (`#351 <https://github.com/ros/ros_comm/issues/351>`_)
|
||||
* ensure to remove delete parameters completely
|
||||
* invalidate cached parent parameters when namespace parameter is set / changes (`#352 <https://github.com/ros/ros_comm/issues/352>`_)
|
||||
* 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>`_)
|
||||
* add AsyncSpinner::canStart() to check if a spinner can be started
|
||||
|
||||
1.11.0 (2014-03-04)
|
||||
-------------------
|
||||
* allow getting parameters with name '/' (`#313 <https://github.com/ros/ros_comm/issues/313>`_)
|
||||
* support for /clock remapping (`#359 <https://github.com/ros/ros_comm/issues/359>`_)
|
||||
* suppress boost::signals deprecation warning (`#362 <https://github.com/ros/ros_comm/issues/362>`_)
|
||||
* use catkin_install_python() to install Python scripts (`#361 <https://github.com/ros/ros_comm/issues/361>`_)
|
||||
|
||||
1.10.0 (2014-02-11)
|
||||
-------------------
|
||||
* remove use of __connection header
|
||||
|
||||
1.9.54 (2014-01-27)
|
||||
-------------------
|
||||
* fix return value of pubUpdate() (`#334 <https://github.com/ros/ros_comm/issues/334>`_)
|
||||
* fix handling optional third xml rpc response argument (`#335 <https://github.com/ros/ros_comm/issues/335>`_)
|
||||
|
||||
1.9.53 (2014-01-14)
|
||||
-------------------
|
||||
|
||||
1.9.52 (2014-01-08)
|
||||
-------------------
|
||||
|
||||
1.9.51 (2014-01-07)
|
||||
-------------------
|
||||
* move several client library independent parts from ros_comm into roscpp_core, split rosbag storage specific stuff from client library usage (`#299 <https://github.com/ros/ros_comm/issues/299>`_)
|
||||
* add missing version dependency on roscpp_core stuff (`#299 <https://github.com/ros/ros_comm/issues/299>`_)
|
||||
* remove log4cxx dependency from roscpp, using new agnostic interface from rosconsole
|
||||
* fix compile problem with gcc 4.4 (`#302 <https://github.com/ros/ros_comm/issues/302>`_)
|
||||
* fix clang warnings
|
||||
* fix usage of boost include directories
|
||||
|
||||
1.9.50 (2013-10-04)
|
||||
-------------------
|
||||
|
||||
1.9.49 (2013-09-16)
|
||||
-------------------
|
||||
* add rosparam getter/setter for std::vector and std::map (`#279 <https://github.com/ros/ros_comm/issues/279>`_)
|
||||
|
||||
1.9.48 (2013-08-21)
|
||||
-------------------
|
||||
|
||||
1.9.47 (2013-07-03)
|
||||
-------------------
|
||||
|
||||
1.9.46 (2013-06-18)
|
||||
-------------------
|
||||
|
||||
1.9.45 (2013-06-06)
|
||||
-------------------
|
||||
* improve handling of UDP transport, when fragmented packets are lost or arive out-of-order the connection is not dropped anymore, onle a single message is lost (`#226 <https://github.com/ros/ros_comm/issues/226>`_)
|
||||
* fix missing generation of constant definitions for services (`ros/gencpp#2 <https://github.com/ros/gencpp/issues/2>`_)
|
||||
* fix restoring thread context when callback throws an exception (`#219 <https://github.com/ros/ros_comm/issues/219>`_)
|
||||
* fix calling PollManager::shutdown() repeatedly (`#217 <https://github.com/ros/ros_comm/issues/217>`_)
|
||||
|
||||
1.9.44 (2013-03-21)
|
||||
-------------------
|
||||
* fix install destination for dll's under Windows
|
||||
|
||||
1.9.43 (2013-03-13)
|
||||
-------------------
|
||||
|
||||
1.9.42 (2013-03-08)
|
||||
-------------------
|
||||
* improve speed of message generation in dry packages (`#183 <https://github.com/ros/ros_comm/issues/183>`_)
|
||||
* fix roscpp service call deadlock (`#149 <https://github.com/ros/ros_comm/issues/149>`_)
|
||||
* fix freezing service calls when returning false (`#168 <https://github.com/ros/ros_comm/issues/168>`_)
|
||||
* fix error message publishing wrong message type (`#178 <https://github.com/ros/ros_comm/issues/178>`_)
|
||||
* fix missing explicit dependency on pthread (`#135 <https://github.com/ros/ros_comm/issues/135>`_)
|
||||
* fix compiler warning about wrong comparison of message md5 hashes (`#165 <https://github.com/ros/ros_comm/issues/165>`_)
|
||||
|
||||
1.9.41 (2013-01-24)
|
||||
-------------------
|
||||
* allow sending data exceeding 2GB in chunks (`#4049 <https://code.ros.org/trac/ros/ticket/4049>`_)
|
||||
* update getParam() doc (`#1460 <https://code.ros.org/trac/ros/ticket/1460>`_)
|
||||
* add param::get(float) (`#3754 <https://code.ros.org/trac/ros/ticket/3754>`_)
|
||||
* update inactive assert when publishing message with md5sum "*", update related tests (`#3714 <https://code.ros.org/trac/ros/ticket/3714>`_)
|
||||
* fix ros master retry timeout (`#4024 <https://code.ros.org/trac/ros/ticket/4024>`_)
|
||||
* fix inactive assert when publishing message with wrong type (`#3714 <https://code.ros.org/trac/ros/ticket/3714>`_)
|
||||
|
||||
1.9.40 (2013-01-13)
|
||||
-------------------
|
||||
|
||||
1.9.39 (2012-12-29)
|
||||
-------------------
|
||||
* first public release for Groovy
|
||||
151
thirdparty/ros/ros_comm/clients/roscpp/CMakeLists.txt
vendored
Normal file
151
thirdparty/ros/ros_comm/clients/roscpp/CMakeLists.txt
vendored
Normal file
@@ -0,0 +1,151 @@
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(roscpp)
|
||||
|
||||
if(NOT WIN32)
|
||||
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Wextra")
|
||||
endif()
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
cpp_common message_generation rosconsole roscpp_serialization roscpp_traits rosgraph_msgs rostime std_msgs xmlrpcpp
|
||||
)
|
||||
|
||||
catkin_package_xml()
|
||||
# split version in parts and pass to extra file
|
||||
string(REPLACE "." ";" roscpp_VERSION_LIST "${roscpp_VERSION}")
|
||||
list(LENGTH roscpp_VERSION_LIST _count)
|
||||
if(NOT _count EQUAL 3)
|
||||
message(FATAL_ERROR "roscpp version '${roscpp_VERSION}' does not match 'MAJOR.MINOR.PATCH' pattern")
|
||||
endif()
|
||||
list(GET roscpp_VERSION_LIST 0 roscpp_VERSION_MAJOR)
|
||||
list(GET roscpp_VERSION_LIST 1 roscpp_VERSION_MINOR)
|
||||
list(GET roscpp_VERSION_LIST 2 roscpp_VERSION_PATCH)
|
||||
|
||||
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/include/ros/common.h.in ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros/common.h)
|
||||
|
||||
find_package(Boost REQUIRED COMPONENTS chrono filesystem signals system)
|
||||
|
||||
include_directories(include ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
|
||||
|
||||
add_message_files(
|
||||
DIRECTORY msg
|
||||
FILES Logger.msg
|
||||
)
|
||||
|
||||
add_service_files(
|
||||
DIRECTORY srv
|
||||
FILES Empty.srv GetLoggers.srv SetLoggerLevel.srv
|
||||
)
|
||||
|
||||
generate_messages()
|
||||
|
||||
set(PTHREAD_LIB "")
|
||||
find_package(Threads)
|
||||
if(CMAKE_THREAD_LIBS_INIT)
|
||||
string(LENGTH ${CMAKE_THREAD_LIBS_INIT} _length)
|
||||
if(_length GREATER 2)
|
||||
string(SUBSTRING ${CMAKE_THREAD_LIBS_INIT} 0 2 _prefix)
|
||||
if(${_prefix} STREQUAL "-l")
|
||||
math(EXPR _rest_len "${_length} - 2")
|
||||
string(SUBSTRING ${CMAKE_THREAD_LIBS_INIT} 2 ${_rest_len} PTHREAD_LIB)
|
||||
endif()
|
||||
endif()
|
||||
endif()
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros
|
||||
LIBRARIES roscpp ${PTHREAD_LIB}
|
||||
CATKIN_DEPENDS cpp_common message_runtime rosconsole roscpp_serialization roscpp_traits rosgraph_msgs rostime std_msgs xmlrpcpp
|
||||
DEPENDS Boost
|
||||
)
|
||||
|
||||
include(CheckIncludeFiles)
|
||||
include(CheckFunctionExists)
|
||||
include(CheckCXXSymbolExists)
|
||||
|
||||
# Not everybody has <ifaddrs.h> (e.g., embedded arm-linux)
|
||||
CHECK_INCLUDE_FILES(ifaddrs.h HAVE_IFADDRS_H)
|
||||
# Not everybody has trunc (e.g., Windows, embedded arm-linux)
|
||||
CHECK_FUNCTION_EXISTS(trunc HAVE_TRUNC)
|
||||
# Not everybody has epoll (e.g., Windows, BSD, embedded arm-linux)
|
||||
CHECK_CXX_SYMBOL_EXISTS(epoll_wait "sys/epoll.h" HAVE_EPOLL)
|
||||
|
||||
# Output test results to config.h
|
||||
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/src/libros/config.h.in ${CMAKE_CURRENT_BINARY_DIR}/config.h)
|
||||
include_directories(${CMAKE_CURRENT_BINARY_DIR})
|
||||
|
||||
add_library(roscpp
|
||||
src/libros/master.cpp
|
||||
src/libros/network.cpp
|
||||
src/libros/subscriber.cpp
|
||||
src/libros/common.cpp
|
||||
src/libros/publisher_link.cpp
|
||||
src/libros/service_publication.cpp
|
||||
src/libros/connection.cpp
|
||||
src/libros/single_subscriber_publisher.cpp
|
||||
src/libros/param.cpp
|
||||
src/libros/service_server.cpp
|
||||
src/libros/wall_timer.cpp
|
||||
src/libros/xmlrpc_manager.cpp
|
||||
src/libros/publisher.cpp
|
||||
src/libros/timer.cpp
|
||||
src/libros/io.cpp
|
||||
src/libros/names.cpp
|
||||
src/libros/topic.cpp
|
||||
src/libros/topic_manager.cpp
|
||||
src/libros/poll_manager.cpp
|
||||
src/libros/publication.cpp
|
||||
src/libros/statistics.cpp
|
||||
src/libros/intraprocess_subscriber_link.cpp
|
||||
src/libros/intraprocess_publisher_link.cpp
|
||||
src/libros/callback_queue.cpp
|
||||
src/libros/service_server_link.cpp
|
||||
src/libros/service_client.cpp
|
||||
src/libros/node_handle.cpp
|
||||
src/libros/connection_manager.cpp
|
||||
src/libros/file_log.cpp
|
||||
src/libros/transport/transport.cpp
|
||||
src/libros/transport/transport_udp.cpp
|
||||
src/libros/transport/transport_tcp.cpp
|
||||
src/libros/subscriber_link.cpp
|
||||
src/libros/service_client_link.cpp
|
||||
src/libros/transport_publisher_link.cpp
|
||||
src/libros/transport_subscriber_link.cpp
|
||||
src/libros/service_manager.cpp
|
||||
src/libros/rosout_appender.cpp
|
||||
src/libros/init.cpp
|
||||
src/libros/subscription.cpp
|
||||
src/libros/subscription_queue.cpp
|
||||
src/libros/spinner.cpp
|
||||
src/libros/internal_timer_manager.cpp
|
||||
src/libros/message_deserializer.cpp
|
||||
src/libros/poll_set.cpp
|
||||
src/libros/service.cpp
|
||||
src/libros/this_node.cpp
|
||||
src/libros/steady_timer.cpp
|
||||
)
|
||||
|
||||
add_dependencies(roscpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
target_link_libraries(roscpp
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
#explicitly install library and includes
|
||||
install(TARGETS roscpp
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
|
||||
install(DIRECTORY include/
|
||||
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h")
|
||||
install(FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros/common.h
|
||||
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/ros)
|
||||
# install legacy infrastructure needed by rosbuild
|
||||
install(FILES rosbuild/roscpp.cmake
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rosbuild)
|
||||
catkin_install_python(PROGRAMS
|
||||
rosbuild/scripts/genmsg_cpp.py
|
||||
rosbuild/scripts/gensrv_cpp.py
|
||||
rosbuild/scripts/msg_gen.py
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rosbuild/scripts)
|
||||
23
thirdparty/ros/ros_comm/clients/roscpp/include/boost_161_condition_variable.h
vendored
Normal file
23
thirdparty/ros/ros_comm/clients/roscpp/include/boost_161_condition_variable.h
vendored
Normal file
@@ -0,0 +1,23 @@
|
||||
#ifndef BOOST_THREAD_CONDITION_VARIABLE_HPP
|
||||
#define BOOST_THREAD_CONDITION_VARIABLE_HPP
|
||||
|
||||
// condition_variable.hpp
|
||||
//
|
||||
// (C) Copyright 2007 Anthony Williams
|
||||
//
|
||||
// Distributed under the Boost Software License, Version 1.0. (See
|
||||
// accompanying file LICENSE_1_0.txt or copy at
|
||||
// http://www.boost.org/LICENSE_1_0.txt)
|
||||
|
||||
#include <boost/thread/detail/platform.hpp>
|
||||
#if defined(BOOST_THREAD_PLATFORM_WIN32)
|
||||
#include <boost/thread/win32/condition_variable.hpp>
|
||||
#elif defined(BOOST_THREAD_PLATFORM_PTHREAD)
|
||||
//#include <boost/thread/pthread/condition_variable.hpp>
|
||||
#include "boost_161_pthread_condition_variable.h"
|
||||
#else
|
||||
#error "Boost threads unavailable on this platform"
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
431
thirdparty/ros/ros_comm/clients/roscpp/include/boost_161_pthread_condition_variable.h
vendored
Normal file
431
thirdparty/ros/ros_comm/clients/roscpp/include/boost_161_pthread_condition_variable.h
vendored
Normal file
@@ -0,0 +1,431 @@
|
||||
#ifndef BOOST_THREAD_CONDITION_VARIABLE_PTHREAD_HPP
|
||||
#define BOOST_THREAD_CONDITION_VARIABLE_PTHREAD_HPP
|
||||
// Distributed under the Boost Software License, Version 1.0. (See
|
||||
// accompanying file LICENSE_1_0.txt or copy at
|
||||
// http://www.boost.org/LICENSE_1_0.txt)
|
||||
// (C) Copyright 2007-10 Anthony Williams
|
||||
// (C) Copyright 2011-2012 Vicente J. Botet Escriba
|
||||
|
||||
// make sure we include our backported version first!!
|
||||
// (before the system version might be included via some of the other header files)
|
||||
#include "boost_161_pthread_condition_variable_fwd.h"
|
||||
|
||||
#include <boost/thread/pthread/timespec.hpp>
|
||||
#include <boost/thread/pthread/pthread_mutex_scoped_lock.hpp>
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
#include <boost/thread/pthread/thread_data.hpp>
|
||||
#endif
|
||||
//#include <boost/thread/pthread/condition_variable_fwd.hpp>
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
#include <boost/chrono/system_clocks.hpp>
|
||||
#include <boost/chrono/ceil.hpp>
|
||||
#endif
|
||||
#include <boost/thread/detail/delete.hpp>
|
||||
|
||||
#include <boost/config/abi_prefix.hpp>
|
||||
|
||||
namespace boost
|
||||
{
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
namespace this_thread
|
||||
{
|
||||
void BOOST_THREAD_DECL interruption_point();
|
||||
}
|
||||
#endif
|
||||
|
||||
namespace thread_cv_detail
|
||||
{
|
||||
template<typename MutexType>
|
||||
struct lock_on_exit
|
||||
{
|
||||
MutexType* m;
|
||||
|
||||
lock_on_exit():
|
||||
m(0)
|
||||
{}
|
||||
|
||||
void activate(MutexType& m_)
|
||||
{
|
||||
m_.unlock();
|
||||
m=&m_;
|
||||
}
|
||||
~lock_on_exit()
|
||||
{
|
||||
if(m)
|
||||
{
|
||||
m->lock();
|
||||
}
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
inline void condition_variable::wait(unique_lock<mutex>& m)
|
||||
{
|
||||
#if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED
|
||||
if(! m.owns_lock())
|
||||
{
|
||||
boost::throw_exception(condition_error(-1, "boost::condition_variable::wait() failed precondition mutex not owned"));
|
||||
}
|
||||
#endif
|
||||
int res=0;
|
||||
{
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
thread_cv_detail::lock_on_exit<unique_lock<mutex> > guard;
|
||||
detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
|
||||
pthread_mutex_t* the_mutex = &internal_mutex;
|
||||
guard.activate(m);
|
||||
#else
|
||||
pthread_mutex_t* the_mutex = m.mutex()->native_handle();
|
||||
#endif
|
||||
res = pthread_cond_wait(&cond,the_mutex);
|
||||
}
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
this_thread::interruption_point();
|
||||
#endif
|
||||
if(res && res != EINTR)
|
||||
{
|
||||
boost::throw_exception(condition_error(res, "boost::condition_variable::wait failed in pthread_cond_wait"));
|
||||
}
|
||||
}
|
||||
|
||||
inline bool condition_variable::do_wait_until(
|
||||
unique_lock<mutex>& m,
|
||||
struct timespec const &timeout)
|
||||
{
|
||||
#if defined BOOST_THREAD_THROW_IF_PRECONDITION_NOT_SATISFIED
|
||||
if (!m.owns_lock())
|
||||
{
|
||||
boost::throw_exception(condition_error(EPERM, "boost::condition_variable::do_wait_until() failed precondition mutex not owned"));
|
||||
}
|
||||
#endif
|
||||
int cond_res;
|
||||
{
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
thread_cv_detail::lock_on_exit<unique_lock<mutex> > guard;
|
||||
detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
|
||||
pthread_mutex_t* the_mutex = &internal_mutex;
|
||||
guard.activate(m);
|
||||
#else
|
||||
pthread_mutex_t* the_mutex = m.mutex()->native_handle();
|
||||
#endif
|
||||
cond_res=pthread_cond_timedwait(&cond,the_mutex,&timeout);
|
||||
}
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
this_thread::interruption_point();
|
||||
#endif
|
||||
if(cond_res==ETIMEDOUT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(cond_res)
|
||||
{
|
||||
boost::throw_exception(condition_error(cond_res, "boost::condition_variable::do_wait_until failed in pthread_cond_timedwait"));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
inline void condition_variable::notify_one() BOOST_NOEXCEPT
|
||||
{
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
|
||||
#endif
|
||||
BOOST_VERIFY(!pthread_cond_signal(&cond));
|
||||
}
|
||||
|
||||
inline void condition_variable::notify_all() BOOST_NOEXCEPT
|
||||
{
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
|
||||
#endif
|
||||
BOOST_VERIFY(!pthread_cond_broadcast(&cond));
|
||||
}
|
||||
|
||||
class condition_variable_any
|
||||
{
|
||||
pthread_mutex_t internal_mutex;
|
||||
pthread_cond_t cond;
|
||||
|
||||
public:
|
||||
BOOST_THREAD_NO_COPYABLE(condition_variable_any)
|
||||
condition_variable_any()
|
||||
{
|
||||
int const res=pthread_mutex_init(&internal_mutex,NULL);
|
||||
if(res)
|
||||
{
|
||||
boost::throw_exception(thread_resource_error(res, "boost::condition_variable_any::condition_variable_any() failed in pthread_mutex_init"));
|
||||
}
|
||||
int const res2 = detail::monotonic_pthread_cond_init(cond);
|
||||
if(res2)
|
||||
{
|
||||
BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex));
|
||||
boost::throw_exception(thread_resource_error(res2, "boost::condition_variable_any::condition_variable_any() failed in detail::monotonic_pthread_cond_init"));
|
||||
}
|
||||
}
|
||||
~condition_variable_any()
|
||||
{
|
||||
BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex));
|
||||
BOOST_VERIFY(!pthread_cond_destroy(&cond));
|
||||
}
|
||||
|
||||
template<typename lock_type>
|
||||
void wait(lock_type& m)
|
||||
{
|
||||
int res=0;
|
||||
{
|
||||
thread_cv_detail::lock_on_exit<lock_type> guard;
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
|
||||
#else
|
||||
boost::pthread::pthread_mutex_scoped_lock check_for_interruption(&internal_mutex);
|
||||
#endif
|
||||
guard.activate(m);
|
||||
res=pthread_cond_wait(&cond,&internal_mutex);
|
||||
}
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
this_thread::interruption_point();
|
||||
#endif
|
||||
if(res)
|
||||
{
|
||||
boost::throw_exception(condition_error(res, "boost::condition_variable_any::wait() failed in pthread_cond_wait"));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename lock_type,typename predicate_type>
|
||||
void wait(lock_type& m,predicate_type pred)
|
||||
{
|
||||
while(!pred()) wait(m);
|
||||
}
|
||||
|
||||
#if defined BOOST_THREAD_USES_DATETIME
|
||||
template<typename lock_type>
|
||||
bool timed_wait(lock_type& m,boost::system_time const& abs_time)
|
||||
{
|
||||
struct timespec const timeout=detail::to_timespec(abs_time);
|
||||
return do_wait_until(m, timeout);
|
||||
}
|
||||
template<typename lock_type>
|
||||
bool timed_wait(lock_type& m,xtime const& abs_time)
|
||||
{
|
||||
return timed_wait(m,system_time(abs_time));
|
||||
}
|
||||
|
||||
template<typename lock_type,typename duration_type>
|
||||
bool timed_wait(lock_type& m,duration_type const& wait_duration)
|
||||
{
|
||||
return timed_wait(m,get_system_time()+wait_duration);
|
||||
}
|
||||
|
||||
template<typename lock_type,typename predicate_type>
|
||||
bool timed_wait(lock_type& m,boost::system_time const& abs_time, predicate_type pred)
|
||||
{
|
||||
while (!pred())
|
||||
{
|
||||
if(!timed_wait(m, abs_time))
|
||||
return pred();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename lock_type,typename predicate_type>
|
||||
bool timed_wait(lock_type& m,xtime const& abs_time, predicate_type pred)
|
||||
{
|
||||
return timed_wait(m,system_time(abs_time),pred);
|
||||
}
|
||||
|
||||
template<typename lock_type,typename duration_type,typename predicate_type>
|
||||
bool timed_wait(lock_type& m,duration_type const& wait_duration,predicate_type pred)
|
||||
{
|
||||
return timed_wait(m,get_system_time()+wait_duration,pred);
|
||||
}
|
||||
#endif
|
||||
#ifndef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
template <class lock_type,class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
lock_type& lock,
|
||||
const chrono::time_point<chrono::system_clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
typedef time_point<system_clock, nanoseconds> nano_sys_tmpt;
|
||||
wait_until(lock,
|
||||
nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
|
||||
return system_clock::now() < t ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class lock_type, class Clock, class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
lock_type& lock,
|
||||
const chrono::time_point<Clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
system_clock::time_point s_now = system_clock::now();
|
||||
typename Clock::time_point c_now = Clock::now();
|
||||
wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
|
||||
return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class lock_type, class Rep, class Period>
|
||||
cv_status
|
||||
wait_for(
|
||||
lock_type& lock,
|
||||
const chrono::duration<Rep, Period>& d)
|
||||
{
|
||||
using namespace chrono;
|
||||
system_clock::time_point s_now = system_clock::now();
|
||||
steady_clock::time_point c_now = steady_clock::now();
|
||||
wait_until(lock, s_now + ceil<nanoseconds>(d));
|
||||
return steady_clock::now() - c_now < d ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
|
||||
}
|
||||
|
||||
template <class lock_type>
|
||||
cv_status wait_until(
|
||||
lock_type& lk,
|
||||
chrono::time_point<chrono::system_clock, chrono::nanoseconds> tp)
|
||||
{
|
||||
using namespace chrono;
|
||||
nanoseconds d = tp.time_since_epoch();
|
||||
timespec ts = boost::detail::to_timespec(d);
|
||||
if (do_wait_until(lk, ts)) return cv_status::no_timeout;
|
||||
else return cv_status::timeout;
|
||||
}
|
||||
#endif
|
||||
#else // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
|
||||
template <class lock_type, class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
lock_type& lock,
|
||||
const chrono::time_point<chrono::steady_clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
typedef time_point<steady_clock, nanoseconds> nano_sys_tmpt;
|
||||
wait_until(lock,
|
||||
nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
|
||||
return steady_clock::now() < t ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class lock_type, class Clock, class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
lock_type& lock,
|
||||
const chrono::time_point<Clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
steady_clock::time_point s_now = steady_clock::now();
|
||||
typename Clock::time_point c_now = Clock::now();
|
||||
wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
|
||||
return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class lock_type, class Rep, class Period>
|
||||
cv_status
|
||||
wait_for(
|
||||
lock_type& lock,
|
||||
const chrono::duration<Rep, Period>& d)
|
||||
{
|
||||
using namespace chrono;
|
||||
steady_clock::time_point c_now = steady_clock::now();
|
||||
wait_until(lock, c_now + ceil<nanoseconds>(d));
|
||||
return steady_clock::now() - c_now < d ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class lock_type>
|
||||
inline cv_status wait_until(
|
||||
lock_type& lock,
|
||||
chrono::time_point<chrono::steady_clock, chrono::nanoseconds> tp)
|
||||
{
|
||||
using namespace chrono;
|
||||
nanoseconds d = tp.time_since_epoch();
|
||||
timespec ts = boost::detail::to_timespec(d);
|
||||
if (do_wait_until(lock, ts)) return cv_status::no_timeout;
|
||||
else return cv_status::timeout;
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
template <class lock_type, class Clock, class Duration, class Predicate>
|
||||
bool
|
||||
wait_until(
|
||||
lock_type& lock,
|
||||
const chrono::time_point<Clock, Duration>& t,
|
||||
Predicate pred)
|
||||
{
|
||||
while (!pred())
|
||||
{
|
||||
if (wait_until(lock, t) == cv_status::timeout)
|
||||
return pred();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
template <class lock_type, class Rep, class Period, class Predicate>
|
||||
bool
|
||||
wait_for(
|
||||
lock_type& lock,
|
||||
const chrono::duration<Rep, Period>& d,
|
||||
Predicate pred)
|
||||
{
|
||||
return wait_until(lock, chrono::steady_clock::now() + d, boost::move(pred));
|
||||
}
|
||||
#endif
|
||||
|
||||
void notify_one() BOOST_NOEXCEPT
|
||||
{
|
||||
boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
|
||||
BOOST_VERIFY(!pthread_cond_signal(&cond));
|
||||
}
|
||||
|
||||
void notify_all() BOOST_NOEXCEPT
|
||||
{
|
||||
boost::pthread::pthread_mutex_scoped_lock internal_lock(&internal_mutex);
|
||||
BOOST_VERIFY(!pthread_cond_broadcast(&cond));
|
||||
}
|
||||
private: // used by boost::thread::try_join_until
|
||||
|
||||
template <class lock_type>
|
||||
bool do_wait_until(
|
||||
lock_type& m,
|
||||
struct timespec const &timeout)
|
||||
{
|
||||
int res=0;
|
||||
{
|
||||
thread_cv_detail::lock_on_exit<lock_type> guard;
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
detail::interruption_checker check_for_interruption(&internal_mutex,&cond);
|
||||
#else
|
||||
boost::pthread::pthread_mutex_scoped_lock check_for_interruption(&internal_mutex);
|
||||
#endif
|
||||
guard.activate(m);
|
||||
res=pthread_cond_timedwait(&cond,&internal_mutex,&timeout);
|
||||
}
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
this_thread::interruption_point();
|
||||
#endif
|
||||
if(res==ETIMEDOUT)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if(res)
|
||||
{
|
||||
boost::throw_exception(condition_error(res, "boost::condition_variable_any::do_wait_until() failed in pthread_cond_timedwait"));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#include <boost/config/abi_suffix.hpp>
|
||||
|
||||
#endif
|
||||
378
thirdparty/ros/ros_comm/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h
vendored
Normal file
378
thirdparty/ros/ros_comm/clients/roscpp/include/boost_161_pthread_condition_variable_fwd.h
vendored
Normal file
@@ -0,0 +1,378 @@
|
||||
#ifndef BOOST_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP
|
||||
#define BOOST_THREAD_PTHREAD_CONDITION_VARIABLE_FWD_HPP
|
||||
// Distributed under the Boost Software License, Version 1.0. (See
|
||||
// accompanying file LICENSE_1_0.txt or copy at
|
||||
// http://www.boost.org/LICENSE_1_0.txt)
|
||||
// (C) Copyright 2007-8 Anthony Williams
|
||||
// (C) Copyright 2011-2012 Vicente J. Botet Escriba
|
||||
|
||||
// define to check if this backported version was included
|
||||
#define USING_BACKPORTED_BOOST_CONDITION_VARIABLE 1
|
||||
|
||||
#include <boost/assert.hpp>
|
||||
#include <boost/throw_exception.hpp>
|
||||
#include <pthread.h>
|
||||
#include <boost/thread/cv_status.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/lock_types.hpp>
|
||||
#include <boost/thread/thread_time.hpp>
|
||||
#include <boost/thread/pthread/timespec.hpp>
|
||||
#if defined BOOST_THREAD_USES_DATETIME
|
||||
#include <boost/thread/xtime.hpp>
|
||||
#endif
|
||||
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
#include <boost/chrono/system_clocks.hpp>
|
||||
#include <boost/chrono/ceil.hpp>
|
||||
#endif
|
||||
#include <boost/thread/detail/delete.hpp>
|
||||
#include <boost/date_time/posix_time/posix_time_duration.hpp>
|
||||
|
||||
#include <boost/config/abi_prefix.hpp>
|
||||
|
||||
namespace boost
|
||||
{
|
||||
namespace detail {
|
||||
inline int monotonic_pthread_cond_init(pthread_cond_t& cond) {
|
||||
|
||||
#ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
pthread_condattr_t attr;
|
||||
int res = pthread_condattr_init(&attr);
|
||||
if (res)
|
||||
{
|
||||
return res;
|
||||
}
|
||||
pthread_condattr_setclock(&attr, CLOCK_MONOTONIC);
|
||||
res=pthread_cond_init(&cond,&attr);
|
||||
pthread_condattr_destroy(&attr);
|
||||
return res;
|
||||
#else
|
||||
return pthread_cond_init(&cond,NULL);
|
||||
#endif
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
class condition_variable
|
||||
{
|
||||
private:
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
pthread_mutex_t internal_mutex;
|
||||
#endif
|
||||
pthread_cond_t cond;
|
||||
|
||||
public:
|
||||
//private: // used by boost::thread::try_join_until
|
||||
|
||||
inline bool do_wait_until(
|
||||
unique_lock<mutex>& lock,
|
||||
struct timespec const &timeout);
|
||||
|
||||
bool do_wait_for(
|
||||
unique_lock<mutex>& lock,
|
||||
struct timespec const &timeout)
|
||||
{
|
||||
#if ! defined BOOST_THREAD_USEFIXES_TIMESPEC
|
||||
return do_wait_until(lock, boost::detail::timespec_plus(timeout, boost::detail::timespec_now()));
|
||||
#elif ! defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
//using namespace chrono;
|
||||
//nanoseconds ns = chrono::system_clock::now().time_since_epoch();
|
||||
|
||||
struct timespec ts = boost::detail::timespec_now_realtime();
|
||||
//ts.tv_sec = static_cast<long>(chrono::duration_cast<chrono::seconds>(ns).count());
|
||||
//ts.tv_nsec = static_cast<long>((ns - chrono::duration_cast<chrono::seconds>(ns)).count());
|
||||
return do_wait_until(lock, boost::detail::timespec_plus(timeout, ts));
|
||||
#else
|
||||
// old behavior was fine for monotonic
|
||||
return do_wait_until(lock, boost::detail::timespec_plus(timeout, boost::detail::timespec_now_realtime()));
|
||||
#endif
|
||||
}
|
||||
|
||||
public:
|
||||
BOOST_THREAD_NO_COPYABLE(condition_variable)
|
||||
condition_variable()
|
||||
{
|
||||
int res;
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
res=pthread_mutex_init(&internal_mutex,NULL);
|
||||
if(res)
|
||||
{
|
||||
boost::throw_exception(thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in pthread_mutex_init"));
|
||||
}
|
||||
#endif
|
||||
res = detail::monotonic_pthread_cond_init(cond);
|
||||
if (res)
|
||||
{
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
BOOST_VERIFY(!pthread_mutex_destroy(&internal_mutex));
|
||||
#endif
|
||||
boost::throw_exception(thread_resource_error(res, "boost::condition_variable::condition_variable() constructor failed in detail::monotonic_pthread_cond_init"));
|
||||
}
|
||||
}
|
||||
~condition_variable()
|
||||
{
|
||||
int ret;
|
||||
#if defined BOOST_THREAD_PROVIDES_INTERRUPTIONS
|
||||
do {
|
||||
ret = pthread_mutex_destroy(&internal_mutex);
|
||||
} while (ret == EINTR);
|
||||
BOOST_ASSERT(!ret);
|
||||
#endif
|
||||
do {
|
||||
ret = pthread_cond_destroy(&cond);
|
||||
} while (ret == EINTR);
|
||||
BOOST_ASSERT(!ret);
|
||||
}
|
||||
|
||||
void wait(unique_lock<mutex>& m);
|
||||
|
||||
template<typename predicate_type>
|
||||
void wait(unique_lock<mutex>& m,predicate_type pred)
|
||||
{
|
||||
while(!pred()) wait(m);
|
||||
}
|
||||
|
||||
#if defined BOOST_THREAD_USES_DATETIME
|
||||
inline bool timed_wait(
|
||||
unique_lock<mutex>& m,
|
||||
boost::system_time const& abs_time)
|
||||
{
|
||||
#if defined BOOST_THREAD_WAIT_BUG
|
||||
struct timespec const timeout=detail::to_timespec(abs_time + BOOST_THREAD_WAIT_BUG);
|
||||
return do_wait_until(m, timeout);
|
||||
#else
|
||||
struct timespec const timeout=detail::to_timespec(abs_time);
|
||||
return do_wait_until(m, timeout);
|
||||
#endif
|
||||
}
|
||||
bool timed_wait(
|
||||
unique_lock<mutex>& m,
|
||||
xtime const& abs_time)
|
||||
{
|
||||
return timed_wait(m,system_time(abs_time));
|
||||
}
|
||||
|
||||
template<typename duration_type>
|
||||
bool timed_wait(
|
||||
unique_lock<mutex>& m,
|
||||
duration_type const& wait_duration)
|
||||
{
|
||||
if (wait_duration.is_pos_infinity())
|
||||
{
|
||||
wait(m); // or do_wait(m,detail::timeout::sentinel());
|
||||
return true;
|
||||
}
|
||||
if (wait_duration.is_special())
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return timed_wait(m,get_system_time()+wait_duration);
|
||||
}
|
||||
|
||||
template<typename predicate_type>
|
||||
bool timed_wait(
|
||||
unique_lock<mutex>& m,
|
||||
boost::system_time const& abs_time,predicate_type pred)
|
||||
{
|
||||
while (!pred())
|
||||
{
|
||||
if(!timed_wait(m, abs_time))
|
||||
return pred();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename predicate_type>
|
||||
bool timed_wait(
|
||||
unique_lock<mutex>& m,
|
||||
xtime const& abs_time,predicate_type pred)
|
||||
{
|
||||
return timed_wait(m,system_time(abs_time),pred);
|
||||
}
|
||||
|
||||
template<typename duration_type,typename predicate_type>
|
||||
bool timed_wait(
|
||||
unique_lock<mutex>& m,
|
||||
duration_type const& wait_duration,predicate_type pred)
|
||||
{
|
||||
if (wait_duration.is_pos_infinity())
|
||||
{
|
||||
while (!pred())
|
||||
{
|
||||
wait(m); // or do_wait(m,detail::timeout::sentinel());
|
||||
}
|
||||
return true;
|
||||
}
|
||||
if (wait_duration.is_special())
|
||||
{
|
||||
return pred();
|
||||
}
|
||||
return timed_wait(m,get_system_time()+wait_duration,pred);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifndef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
|
||||
template <class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::time_point<chrono::system_clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
typedef time_point<system_clock, nanoseconds> nano_sys_tmpt;
|
||||
wait_until(lock,
|
||||
nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
|
||||
return system_clock::now() < t ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class Clock, class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::time_point<Clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
system_clock::time_point s_now = system_clock::now();
|
||||
typename Clock::time_point c_now = Clock::now();
|
||||
wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
|
||||
return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
|
||||
}
|
||||
|
||||
|
||||
|
||||
template <class Rep, class Period>
|
||||
cv_status
|
||||
wait_for(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::duration<Rep, Period>& d)
|
||||
{
|
||||
using namespace chrono;
|
||||
system_clock::time_point s_now = system_clock::now();
|
||||
steady_clock::time_point c_now = steady_clock::now();
|
||||
wait_until(lock, s_now + ceil<nanoseconds>(d));
|
||||
return steady_clock::now() - c_now < d ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
|
||||
}
|
||||
|
||||
inline cv_status wait_until(
|
||||
unique_lock<mutex>& lk,
|
||||
chrono::time_point<chrono::system_clock, chrono::nanoseconds> tp)
|
||||
{
|
||||
using namespace chrono;
|
||||
nanoseconds d = tp.time_since_epoch();
|
||||
timespec ts = boost::detail::to_timespec(d);
|
||||
if (do_wait_until(lk, ts)) return cv_status::no_timeout;
|
||||
else return cv_status::timeout;
|
||||
}
|
||||
#endif
|
||||
|
||||
#else // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
|
||||
template <class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::time_point<chrono::steady_clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
typedef time_point<steady_clock, nanoseconds> nano_sys_tmpt;
|
||||
wait_until(lock,
|
||||
nano_sys_tmpt(ceil<nanoseconds>(t.time_since_epoch())));
|
||||
return steady_clock::now() < t ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class Clock, class Duration>
|
||||
cv_status
|
||||
wait_until(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::time_point<Clock, Duration>& t)
|
||||
{
|
||||
using namespace chrono;
|
||||
steady_clock::time_point s_now = steady_clock::now();
|
||||
typename Clock::time_point c_now = Clock::now();
|
||||
wait_until(lock, s_now + ceil<nanoseconds>(t - c_now));
|
||||
return Clock::now() < t ? cv_status::no_timeout : cv_status::timeout;
|
||||
}
|
||||
|
||||
template <class Rep, class Period>
|
||||
cv_status
|
||||
wait_for(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::duration<Rep, Period>& d)
|
||||
{
|
||||
using namespace chrono;
|
||||
steady_clock::time_point c_now = steady_clock::now();
|
||||
wait_until(lock, c_now + ceil<nanoseconds>(d));
|
||||
return steady_clock::now() - c_now < d ? cv_status::no_timeout :
|
||||
cv_status::timeout;
|
||||
}
|
||||
|
||||
inline cv_status wait_until(
|
||||
unique_lock<mutex>& lk,
|
||||
chrono::time_point<chrono::steady_clock, chrono::nanoseconds> tp)
|
||||
{
|
||||
using namespace chrono;
|
||||
nanoseconds d = tp.time_since_epoch();
|
||||
timespec ts = boost::detail::to_timespec(d);
|
||||
if (do_wait_until(lk, ts)) return cv_status::no_timeout;
|
||||
else return cv_status::timeout;
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // defined BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
|
||||
#ifdef BOOST_THREAD_USES_CHRONO
|
||||
template <class Clock, class Duration, class Predicate>
|
||||
bool
|
||||
wait_until(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::time_point<Clock, Duration>& t,
|
||||
Predicate pred)
|
||||
{
|
||||
while (!pred())
|
||||
{
|
||||
if (wait_until(lock, t) == cv_status::timeout)
|
||||
return pred();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
template <class Rep, class Period, class Predicate>
|
||||
bool
|
||||
wait_for(
|
||||
unique_lock<mutex>& lock,
|
||||
const chrono::duration<Rep, Period>& d,
|
||||
Predicate pred)
|
||||
{
|
||||
return wait_until(lock, chrono::steady_clock::now() + d, boost::move(pred));
|
||||
}
|
||||
#endif
|
||||
|
||||
#define BOOST_THREAD_DEFINES_CONDITION_VARIABLE_NATIVE_HANDLE
|
||||
typedef pthread_cond_t* native_handle_type;
|
||||
native_handle_type native_handle()
|
||||
{
|
||||
return &cond;
|
||||
}
|
||||
|
||||
void notify_one() BOOST_NOEXCEPT;
|
||||
void notify_all() BOOST_NOEXCEPT;
|
||||
|
||||
|
||||
};
|
||||
|
||||
BOOST_THREAD_DECL void notify_all_at_thread_exit(condition_variable& cond, unique_lock<mutex> lk);
|
||||
|
||||
}
|
||||
|
||||
|
||||
#include <boost/config/abi_suffix.hpp>
|
||||
|
||||
#endif
|
||||
165
thirdparty/ros/ros_comm/clients/roscpp/include/ros/advertise_options.h
vendored
Normal file
165
thirdparty/ros/ros_comm/clients/roscpp/include/ros/advertise_options.h
vendored
Normal file
@@ -0,0 +1,165 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_ADVERTISE_OPTIONS_H
|
||||
#define ROSCPP_ADVERTISE_OPTIONS_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/message_traits.h"
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Encapsulates all options available for creating a Publisher
|
||||
*/
|
||||
struct ROSCPP_DECL AdvertiseOptions
|
||||
{
|
||||
AdvertiseOptions()
|
||||
: callback_queue(0)
|
||||
, latch(false)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Constructor
|
||||
* \param _topic Topic to publish on
|
||||
* \param _queue_size Maximum number of outgoing messages to be queued for delivery to subscribers
|
||||
* \param _md5sum The md5sum of the message datatype published on this topic
|
||||
* \param _datatype Datatype of the message published on this topic (eg. "std_msgs/String")
|
||||
* \param _connect_cb Function to call when a subscriber connects to this topic
|
||||
* \param _disconnect_cb Function to call when a subscriber disconnects from this topic
|
||||
*/
|
||||
AdvertiseOptions(const std::string& _topic, uint32_t _queue_size, const std::string& _md5sum,
|
||||
const std::string& _datatype, const std::string& _message_definition,
|
||||
const SubscriberStatusCallback& _connect_cb = SubscriberStatusCallback(),
|
||||
const SubscriberStatusCallback& _disconnect_cb = SubscriberStatusCallback())
|
||||
: topic(_topic)
|
||||
, queue_size(_queue_size)
|
||||
, md5sum(_md5sum)
|
||||
, datatype(_datatype)
|
||||
, message_definition(_message_definition)
|
||||
, connect_cb(_connect_cb)
|
||||
, disconnect_cb(_disconnect_cb)
|
||||
, callback_queue(0)
|
||||
, latch(false)
|
||||
, has_header(false)
|
||||
{}
|
||||
|
||||
/**
|
||||
* \brief templated helper function for automatically filling out md5sum, datatype and message definition
|
||||
*
|
||||
* \param M [template] Message type
|
||||
* \param _topic Topic to publish on
|
||||
* \param _queue_size Maximum number of outgoing messages to be queued for delivery to subscribers
|
||||
* \param _connect_cb Function to call when a subscriber connects to this topic
|
||||
* \param _disconnect_cb Function to call when a subscriber disconnects from this topic
|
||||
*/
|
||||
template <class M>
|
||||
void init(const std::string& _topic, uint32_t _queue_size,
|
||||
const SubscriberStatusCallback& _connect_cb = SubscriberStatusCallback(),
|
||||
const SubscriberStatusCallback& _disconnect_cb = SubscriberStatusCallback())
|
||||
{
|
||||
topic = _topic;
|
||||
queue_size = _queue_size;
|
||||
connect_cb = _connect_cb;
|
||||
disconnect_cb = _disconnect_cb;
|
||||
md5sum = message_traits::md5sum<M>();
|
||||
datatype = message_traits::datatype<M>();
|
||||
message_definition = message_traits::definition<M>();
|
||||
has_header = message_traits::hasHeader<M>();
|
||||
}
|
||||
|
||||
std::string topic; ///< The topic to publish on
|
||||
uint32_t queue_size; ///< The maximum number of outgoing messages to be queued for delivery to subscribers
|
||||
|
||||
std::string md5sum; ///< The md5sum of the message datatype published on this topic
|
||||
std::string datatype; ///< The datatype of the message published on this topic (eg. "std_msgs/String")
|
||||
std::string message_definition; ///< The full definition of the message published on this topic
|
||||
|
||||
SubscriberStatusCallback connect_cb; ///< The function to call when a subscriber connects to this topic
|
||||
SubscriberStatusCallback disconnect_cb; ///< The function to call when a subscriber disconnects from this topic
|
||||
|
||||
CallbackQueueInterface* callback_queue; ///< Queue to add callbacks to. If NULL, the global callback queue will be used
|
||||
|
||||
/**
|
||||
* \brief An object whose destruction will prevent the callbacks associated with this advertisement from being called
|
||||
*
|
||||
* A shared pointer to an object to track for these callbacks. If set, the a weak_ptr will be created to this object,
|
||||
* and if the reference count goes to 0 the subscriber callbacks will not get called.
|
||||
*
|
||||
* \note Note that setting this will cause a new reference to be added to the object before the
|
||||
* callback, and for it to go out of scope (and potentially be deleted) in the code path (and therefore
|
||||
* thread) that the callback is invoked from.
|
||||
*/
|
||||
VoidConstPtr tracked_object;
|
||||
|
||||
/**
|
||||
* \brief Whether or not this publication should "latch". A latching publication will automatically send out the last published message
|
||||
* to any new subscribers.
|
||||
*/
|
||||
bool latch;
|
||||
|
||||
/** \brief Tells whether or not the message has a header. If it does, the sequence number will be written directly into the
|
||||
* serialized bytes after the message has been serialized.
|
||||
*/
|
||||
bool has_header;
|
||||
|
||||
|
||||
/**
|
||||
* \brief Templated helper function for creating an AdvertiseOptions for a message type with most options.
|
||||
*
|
||||
* \param M [template] Message type
|
||||
* \param topic Topic to publish on
|
||||
* \param queue_size Maximum number of outgoing messages to be queued for delivery to subscribers
|
||||
* \param connect_cb Function to call when a subscriber connects to this topic
|
||||
* \param disconnect_cb Function to call when a subscriber disconnects from this topic
|
||||
* \param tracked_object tracked object to use (see AdvertiseOptions::tracked_object)
|
||||
* \param queue The callback queue to use (see AdvertiseOptions::callback_queue)
|
||||
*
|
||||
* \return an AdvertiseOptions which embodies the parameters
|
||||
*/
|
||||
template<class M>
|
||||
static AdvertiseOptions create(const std::string& topic, uint32_t queue_size,
|
||||
const SubscriberStatusCallback& connect_cb,
|
||||
const SubscriberStatusCallback& disconnect_cb,
|
||||
const VoidConstPtr& tracked_object,
|
||||
CallbackQueueInterface* queue)
|
||||
{
|
||||
AdvertiseOptions ops;
|
||||
ops.init<M>(topic, queue_size, connect_cb, disconnect_cb);
|
||||
ops.tracked_object = tracked_object;
|
||||
ops.callback_queue = queue;
|
||||
return ops;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
166
thirdparty/ros/ros_comm/clients/roscpp/include/ros/advertise_service_options.h
vendored
Normal file
166
thirdparty/ros/ros_comm/clients/roscpp/include/ros/advertise_service_options.h
vendored
Normal file
@@ -0,0 +1,166 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_ADVERTISE_SERVICE_OPTIONS_H
|
||||
#define ROSCPP_ADVERTISE_SERVICE_OPTIONS_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/service_callback_helper.h"
|
||||
#include "ros/service_traits.h"
|
||||
#include "ros/message_traits.h"
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Encapsulates all options available for creating a ServiceServer
|
||||
*/
|
||||
struct ROSCPP_DECL AdvertiseServiceOptions
|
||||
{
|
||||
AdvertiseServiceOptions()
|
||||
: callback_queue(0)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Templated convenience method for filling out md5sum/etc. based on the service request/response types
|
||||
* \param _service Service name to advertise on
|
||||
* \param _callback Callback to call when this service is called
|
||||
*/
|
||||
template<class MReq, class MRes>
|
||||
void init(const std::string& _service, const boost::function<bool(MReq&, MRes&)>& _callback)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
namespace mt = message_traits;
|
||||
if (st::md5sum<MReq>() != st::md5sum<MRes>())
|
||||
{
|
||||
ROS_FATAL("the request and response parameters to the server "
|
||||
"callback function must be autogenerated from the same "
|
||||
"server definition file (.srv). your advertise_servce "
|
||||
"call for %s appeared to use request/response types "
|
||||
"from different .srv files.", service.c_str());
|
||||
ROS_BREAK();
|
||||
}
|
||||
|
||||
service = _service;
|
||||
md5sum = st::md5sum<MReq>();
|
||||
datatype = st::datatype<MReq>();
|
||||
req_datatype = mt::datatype<MReq>();
|
||||
res_datatype = mt::datatype<MRes>();
|
||||
helper = boost::make_shared<ServiceCallbackHelperT<ServiceSpec<MReq, MRes> > >(_callback);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Templated convenience method for filling out md5sum/etc. based on the service type
|
||||
* \param _service Service name to advertise on
|
||||
* \param _callback Callback to call when this service is called
|
||||
*/
|
||||
template<class Service>
|
||||
void init(const std::string& _service, const boost::function<bool(typename Service::Request&, typename Service::Response&)>& _callback)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
namespace mt = message_traits;
|
||||
typedef typename Service::Request Request;
|
||||
typedef typename Service::Response Response;
|
||||
service = _service;
|
||||
md5sum = st::md5sum<Service>();
|
||||
datatype = st::datatype<Service>();
|
||||
req_datatype = mt::datatype<Request>();
|
||||
res_datatype = mt::datatype<Response>();
|
||||
helper = boost::make_shared<ServiceCallbackHelperT<ServiceSpec<Request, Response> > >(_callback);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Templated convenience method for filling out md5sum/etc. based on the service spec type
|
||||
* \param _service Service name to advertise on
|
||||
* \param _callback Callback to call when this service is called
|
||||
*/
|
||||
template<class Spec>
|
||||
void initBySpecType(const std::string& _service, const typename Spec::CallbackType& _callback)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
namespace mt = message_traits;
|
||||
typedef typename Spec::RequestType Request;
|
||||
typedef typename Spec::ResponseType Response;
|
||||
service = _service;
|
||||
md5sum = st::md5sum<Request>();
|
||||
datatype = st::datatype<Request>();
|
||||
req_datatype = mt::datatype<Request>();
|
||||
res_datatype = mt::datatype<Response>();
|
||||
helper = boost::make_shared<ServiceCallbackHelperT<Spec> >(_callback);
|
||||
}
|
||||
|
||||
std::string service; ///< Service name
|
||||
std::string md5sum; ///< MD5 of the service
|
||||
std::string datatype; ///< Datatype of the service
|
||||
std::string req_datatype; ///< Request message datatype
|
||||
std::string res_datatype; ///< Response message datatype
|
||||
|
||||
ServiceCallbackHelperPtr helper; ///< Helper object used for creating messages and calling callbacks
|
||||
|
||||
CallbackQueueInterface* callback_queue; ///< Queue to add callbacks to. If NULL, the global callback queue will be used
|
||||
|
||||
/**
|
||||
* \brief An object whose destruction will prevent the callback associated with this service from being called
|
||||
*
|
||||
* A shared pointer to an object to track for these callbacks. If set, the a weak_ptr will be created to this object,
|
||||
* and if the reference count goes to 0 the subscriber callbacks will not get called.
|
||||
*
|
||||
* \note Note that setting this will cause a new reference to be added to the object before the
|
||||
* callback, and for it to go out of scope (and potentially be deleted) in the code path (and therefore
|
||||
* thread) that the callback is invoked from.
|
||||
*/
|
||||
VoidConstPtr tracked_object;
|
||||
|
||||
/**
|
||||
* \brief Templated helper function for creating an AdvertiseServiceOptions with all of its options
|
||||
* \param service Service name to advertise on
|
||||
* \param callback The callback to invoke when the service is called
|
||||
* \param tracked_object The tracked object to use (see AdvertiseServiceOptions::tracked_object)
|
||||
* \param queue The callback queue to use (see AdvertiseServiceOptions::callback_queue)
|
||||
*/
|
||||
template<class Service>
|
||||
static AdvertiseServiceOptions create(const std::string& service,
|
||||
const boost::function<bool(typename Service::Request&, typename Service::Response&)>& callback,
|
||||
const VoidConstPtr& tracked_object,
|
||||
CallbackQueueInterface* queue)
|
||||
{
|
||||
AdvertiseServiceOptions ops;
|
||||
ops.init<typename Service::Request, typename Service::Response>(service, callback);
|
||||
ops.tracked_object = tracked_object;
|
||||
ops.callback_queue = queue;
|
||||
return ops;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
203
thirdparty/ros/ros_comm/clients/roscpp/include/ros/callback_queue.h
vendored
Normal file
203
thirdparty/ros/ros_comm/clients/roscpp/include/ros/callback_queue.h
vendored
Normal file
@@ -0,0 +1,203 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_CALLBACK_QUEUE_H
|
||||
#define ROSCPP_CALLBACK_QUEUE_H
|
||||
|
||||
// check if we might need to include our own backported version boost::condition_variable
|
||||
// in order to use CLOCK_MONOTONIC for the condition variable
|
||||
// the include order here is important!
|
||||
#ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#include <boost/version.hpp>
|
||||
#if BOOST_VERSION < 106100
|
||||
// use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377
|
||||
#include "boost_161_condition_variable.h"
|
||||
#else // Boost version is 1.61 or greater and has the steady clock fixes
|
||||
#include <boost/thread/condition_variable.hpp>
|
||||
#endif
|
||||
#else // !BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#include <boost/thread/condition_variable.hpp>
|
||||
#endif // BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
|
||||
#include "ros/callback_queue_interface.h"
|
||||
#include "ros/time.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/shared_mutex.hpp>
|
||||
#include <boost/thread/tss.hpp>
|
||||
|
||||
#include <list>
|
||||
#include <deque>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief This is the default implementation of the ros::CallbackQueueInterface
|
||||
*/
|
||||
class ROSCPP_DECL CallbackQueue : public CallbackQueueInterface
|
||||
{
|
||||
public:
|
||||
CallbackQueue(bool enabled = true);
|
||||
virtual ~CallbackQueue();
|
||||
|
||||
virtual void addCallback(const CallbackInterfacePtr& callback, uint64_t removal_id = 0);
|
||||
virtual void removeByID(uint64_t removal_id);
|
||||
|
||||
enum CallOneResult
|
||||
{
|
||||
Called,
|
||||
TryAgain,
|
||||
Disabled,
|
||||
Empty,
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Pop a single callback off the front of the queue and invoke it. If the callback was not ready to be called,
|
||||
* pushes it back onto the queue.
|
||||
*/
|
||||
CallOneResult callOne()
|
||||
{
|
||||
return callOne(ros::WallDuration());
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Pop a single callback off the front of the queue and invoke it. If the callback was not ready to be called,
|
||||
* pushes it back onto the queue. This version includes a timeout which lets you specify the amount of time to wait for
|
||||
* a callback to be available before returning.
|
||||
*
|
||||
* \param timeout The amount of time to wait for a callback to be available. If there is already a callback available,
|
||||
* this parameter does nothing.
|
||||
*/
|
||||
CallOneResult callOne(ros::WallDuration timeout);
|
||||
|
||||
/**
|
||||
* \brief Invoke all callbacks currently in the queue. If a callback was not ready to be called, pushes it back onto the queue.
|
||||
*/
|
||||
void callAvailable()
|
||||
{
|
||||
callAvailable(ros::WallDuration());
|
||||
}
|
||||
/**
|
||||
* \brief Invoke all callbacks currently in the queue. If a callback was not ready to be called, pushes it back onto the queue. This version
|
||||
* includes a timeout which lets you specify the amount of time to wait for a callback to be available before returning.
|
||||
*
|
||||
* \param timeout The amount of time to wait for at least one callback to be available. If there is already at least one callback available,
|
||||
* this parameter does nothing.
|
||||
*/
|
||||
void callAvailable(ros::WallDuration timeout);
|
||||
|
||||
/**
|
||||
* \brief returns whether or not the queue is empty
|
||||
*/
|
||||
bool empty() { return isEmpty(); }
|
||||
/**
|
||||
* \brief returns whether or not the queue is empty
|
||||
*/
|
||||
bool isEmpty();
|
||||
/**
|
||||
* \brief Removes all callbacks from the queue. Does \b not wait for calls currently in progress to finish.
|
||||
*/
|
||||
void clear();
|
||||
|
||||
/**
|
||||
* \brief Enable the queue (queue is enabled by default)
|
||||
*/
|
||||
void enable();
|
||||
/**
|
||||
* \brief Disable the queue, meaning any calls to addCallback() will have no effect
|
||||
*/
|
||||
void disable();
|
||||
/**
|
||||
* \brief Returns whether or not this queue is enabled
|
||||
*/
|
||||
bool isEnabled();
|
||||
|
||||
protected:
|
||||
void setupTLS();
|
||||
|
||||
struct TLS;
|
||||
CallOneResult callOneCB(TLS* tls);
|
||||
|
||||
struct IDInfo
|
||||
{
|
||||
uint64_t id;
|
||||
boost::shared_mutex calling_rw_mutex;
|
||||
};
|
||||
typedef boost::shared_ptr<IDInfo> IDInfoPtr;
|
||||
typedef std::map<uint64_t, IDInfoPtr> M_IDInfo;
|
||||
|
||||
IDInfoPtr getIDInfo(uint64_t id);
|
||||
|
||||
struct CallbackInfo
|
||||
{
|
||||
CallbackInfo()
|
||||
: removal_id(0)
|
||||
, marked_for_removal(false)
|
||||
{}
|
||||
CallbackInterfacePtr callback;
|
||||
uint64_t removal_id;
|
||||
bool marked_for_removal;
|
||||
};
|
||||
typedef std::list<CallbackInfo> L_CallbackInfo;
|
||||
typedef std::deque<CallbackInfo> D_CallbackInfo;
|
||||
D_CallbackInfo callbacks_;
|
||||
size_t calling_;
|
||||
boost::mutex mutex_;
|
||||
boost::condition_variable condition_;
|
||||
|
||||
boost::mutex id_info_mutex_;
|
||||
M_IDInfo id_info_;
|
||||
|
||||
struct TLS
|
||||
{
|
||||
TLS()
|
||||
: calling_in_this_thread(0xffffffffffffffffULL)
|
||||
, cb_it(callbacks.end())
|
||||
{}
|
||||
uint64_t calling_in_this_thread;
|
||||
D_CallbackInfo callbacks;
|
||||
D_CallbackInfo::iterator cb_it;
|
||||
};
|
||||
boost::thread_specific_ptr<TLS> tls_;
|
||||
|
||||
bool enabled_;
|
||||
};
|
||||
typedef boost::shared_ptr<CallbackQueue> CallbackQueuePtr;
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
101
thirdparty/ros/ros_comm/clients/roscpp/include/ros/callback_queue_interface.h
vendored
Normal file
101
thirdparty/ros/ros_comm/clients/roscpp/include/ros/callback_queue_interface.h
vendored
Normal file
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_CALLBACK_QUEUE_INTERFACE_H
|
||||
#define ROSCPP_CALLBACK_QUEUE_INTERFACE_H
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include "common.h"
|
||||
#include "ros/types.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Abstract interface for items which can be added to a CallbackQueueInterface
|
||||
*/
|
||||
class ROSCPP_DECL CallbackInterface
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* \brief Possible results for the call() method
|
||||
*/
|
||||
enum CallResult
|
||||
{
|
||||
Success, ///< Call succeeded
|
||||
TryAgain, ///< Call not ready, try again later
|
||||
Invalid, ///< Call no longer valid
|
||||
};
|
||||
|
||||
virtual ~CallbackInterface() {}
|
||||
|
||||
/**
|
||||
* \brief Call this callback
|
||||
* \return The result of the call
|
||||
*/
|
||||
virtual CallResult call() = 0;
|
||||
/**
|
||||
* \brief Provides the opportunity for specifying that a callback is not ready to be called
|
||||
* before call() actually takes place.
|
||||
*/
|
||||
virtual bool ready() { return true; }
|
||||
};
|
||||
typedef boost::shared_ptr<CallbackInterface> CallbackInterfacePtr;
|
||||
|
||||
/**
|
||||
* \brief Abstract interface for a queue used to handle all callbacks within roscpp.
|
||||
*
|
||||
* Allows you to inherit and provide your own implementation that can be used instead of our
|
||||
* default CallbackQueue
|
||||
*/
|
||||
class CallbackQueueInterface
|
||||
{
|
||||
public:
|
||||
virtual ~CallbackQueueInterface() {}
|
||||
|
||||
/**
|
||||
* \brief Add a callback, with an optional owner id. The owner id can be used to
|
||||
* remove a set of callbacks from this queue.
|
||||
*/
|
||||
virtual void addCallback(const CallbackInterfacePtr& callback, uint64_t owner_id = 0) = 0;
|
||||
|
||||
/**
|
||||
* \brief Remove all callbacks associated with an owner id
|
||||
*/
|
||||
virtual void removeByID(uint64_t owner_id) = 0;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
73
thirdparty/ros/ros_comm/clients/roscpp/include/ros/common.h.in
vendored
Normal file
73
thirdparty/ros/ros_comm/clients/roscpp/include/ros/common.h.in
vendored
Normal file
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_COMMON_H
|
||||
#define ROSCPP_COMMON_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <assert.h>
|
||||
#include <stddef.h>
|
||||
#include <string>
|
||||
|
||||
#include "ros/assert.h"
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/serialized_message.h"
|
||||
|
||||
#include <boost/shared_array.hpp>
|
||||
|
||||
#define ROS_VERSION_MAJOR @roscpp_VERSION_MAJOR@
|
||||
#define ROS_VERSION_MINOR @roscpp_VERSION_MINOR@
|
||||
#define ROS_VERSION_PATCH @roscpp_VERSION_PATCH@
|
||||
#define ROS_VERSION_COMBINED(major, minor, patch) (((major) << 20) | ((minor) << 10) | (patch))
|
||||
#define ROS_VERSION ROS_VERSION_COMBINED(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH)
|
||||
|
||||
#define ROS_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (ROS_VERSION_COMBINED(major1, minor1, patch1) >= ROS_VERSION_COMBINED(major2, minor2, patch2))
|
||||
#define ROS_VERSION_MINIMUM(major, minor, patch) ROS_VERSION_GE(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH, major, minor, patch)
|
||||
|
||||
#include <ros/macros.h>
|
||||
|
||||
// Import/export for windows dll's and visibility for gcc shared libraries.
|
||||
|
||||
#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
|
||||
#ifdef roscpp_EXPORTS // we are building a shared lib/dll
|
||||
#define ROSCPP_DECL ROS_HELPER_EXPORT
|
||||
#else // we are using shared lib/dll
|
||||
#define ROSCPP_DECL ROS_HELPER_IMPORT
|
||||
#endif
|
||||
#else // ros is being built around static libraries
|
||||
#define ROSCPP_DECL
|
||||
#endif
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
void disableAllSignalsInThisThread();
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
271
thirdparty/ros/ros_comm/clients/roscpp/include/ros/connection.h
vendored
Normal file
271
thirdparty/ros/ros_comm/clients/roscpp/include/ros/connection.h
vendored
Normal file
@@ -0,0 +1,271 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_CONNECTION_H
|
||||
#define ROSCPP_CONNECTION_H
|
||||
|
||||
#include "ros/header.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/signals2.hpp>
|
||||
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
#define READ_BUFFER_SIZE (1024*64)
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class Transport;
|
||||
typedef boost::shared_ptr<Transport> TransportPtr;
|
||||
class Connection;
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
typedef boost::function<void(const ConnectionPtr&, const boost::shared_array<uint8_t>&, uint32_t, bool)> ReadFinishedFunc;
|
||||
typedef boost::function<void(const ConnectionPtr&)> WriteFinishedFunc;
|
||||
|
||||
typedef boost::function<bool(const ConnectionPtr&, const Header&)> HeaderReceivedFunc;
|
||||
|
||||
/**
|
||||
* \brief Encapsulates a connection to a remote host, independent of the transport type
|
||||
*
|
||||
* Connection provides automatic header negotiation, as well as easy ways of reading and writing
|
||||
* arbitrary amounts of data without having to set up your own state machines.
|
||||
*/
|
||||
class ROSCPP_DECL Connection : public boost::enable_shared_from_this<Connection>
|
||||
{
|
||||
public:
|
||||
enum DropReason
|
||||
{
|
||||
TransportDisconnect,
|
||||
HeaderError,
|
||||
Destructing,
|
||||
};
|
||||
|
||||
Connection();
|
||||
~Connection();
|
||||
|
||||
/**
|
||||
* \brief Initialize this connection.
|
||||
*/
|
||||
void initialize(const TransportPtr& transport, bool is_server, const HeaderReceivedFunc& header_func);
|
||||
/**
|
||||
* \brief Drop this connection. Anything added as a drop listener through addDropListener will get called back when this connection has
|
||||
* been dropped.
|
||||
*/
|
||||
void drop(DropReason reason);
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not this connection has been dropped
|
||||
*/
|
||||
bool isDropped();
|
||||
|
||||
/**
|
||||
* \brief Returns true if we're currently sending a header error (and will be automatically dropped when it's finished)
|
||||
*/
|
||||
bool isSendingHeaderError() { return sending_header_error_; }
|
||||
|
||||
/**
|
||||
* \brief Send a header error message, of the form "error=<message>". Drops the connection once the data has written successfully (or fails to write)
|
||||
* \param error_message The error message
|
||||
*/
|
||||
void sendHeaderError(const std::string& error_message);
|
||||
/**
|
||||
* \brief Send a list of string key/value pairs as a header message.
|
||||
* \param key_vals The values to send. Neither keys nor values can have any newlines in them
|
||||
* \param finished_callback The function to call when the header has finished writing
|
||||
*/
|
||||
void writeHeader(const M_string& key_vals, const WriteFinishedFunc& finished_callback);
|
||||
|
||||
/**
|
||||
* \brief Read a number of bytes, calling a callback when finished
|
||||
*
|
||||
* read() will not queue up multiple reads. Once read() has been called, it is not valid to call it again until the
|
||||
* finished callback has been called. It \b is valid to call read() from within the finished callback.
|
||||
*
|
||||
* The finished callback is of the form void(const ConnectionPtr&, const boost::shared_array<uint8_t>&, uint32_t)
|
||||
*
|
||||
* \note The finished callback may be called from within this call to read() if the data has already arrived
|
||||
*
|
||||
* \param size The size, in bytes, of data to read
|
||||
* \param finished_callback The function to call when this read is finished
|
||||
*/
|
||||
void read(uint32_t size, const ReadFinishedFunc& finished_callback);
|
||||
/**
|
||||
* \brief Write a buffer of bytes, calling a callback when finished
|
||||
*
|
||||
* write() will not queue up multiple writes. Once write() has been called, it is not valid to call it again until
|
||||
* the finished callback has been called. It \b is valid to call write() from within the finished callback.
|
||||
*
|
||||
* * The finished callback is of the form void(const ConnectionPtr&)
|
||||
*
|
||||
* \note The finished callback may be called from within this call to write() if the data can be written immediately
|
||||
*
|
||||
* \param buffer The buffer of data to write
|
||||
* \param size The size of the buffer, in bytes
|
||||
* \param finished_callback The function to call when the write has finished
|
||||
* \param immediate Whether to immediately try to write as much data as possible to the socket or to pass
|
||||
* the data off to the server thread
|
||||
*/
|
||||
void write(const boost::shared_array<uint8_t>& buffer, uint32_t size, const WriteFinishedFunc& finished_callback, bool immedate = true);
|
||||
|
||||
typedef boost::signals2::signal<void(const ConnectionPtr&, DropReason reason)> DropSignal;
|
||||
typedef boost::function<void(const ConnectionPtr&, DropReason reason)> DropFunc;
|
||||
/**
|
||||
* \brief Add a callback to be called when this connection has dropped
|
||||
*/
|
||||
boost::signals2::connection addDropListener(const DropFunc& slot);
|
||||
void removeDropListener(const boost::signals2::connection& c);
|
||||
|
||||
/**
|
||||
* \brief Set the header receipt callback
|
||||
*/
|
||||
void setHeaderReceivedCallback(const HeaderReceivedFunc& func);
|
||||
|
||||
/**
|
||||
* \brief Get the Transport associated with this connection
|
||||
*/
|
||||
const TransportPtr& getTransport() { return transport_; }
|
||||
/**
|
||||
* \brief Get the Header associated with this connection
|
||||
*/
|
||||
Header& getHeader() { return header_; }
|
||||
|
||||
/**
|
||||
* \brief Set the Header associated with this connection (used with UDPROS,
|
||||
* which receives the connection during XMLRPC negotiation).
|
||||
*/
|
||||
void setHeader(const Header& header) { header_ = header; }
|
||||
|
||||
std::string getCallerId();
|
||||
std::string getRemoteString();
|
||||
|
||||
private:
|
||||
/**
|
||||
* \brief Called by the Transport when there is data available to be read
|
||||
*/
|
||||
void onReadable(const TransportPtr& transport);
|
||||
/**
|
||||
* \brief Called by the Transport when it is possible to write data
|
||||
*/
|
||||
void onWriteable(const TransportPtr& transport);
|
||||
/**
|
||||
* \brief Called by the Transport when it has been disconnected, either through a call to close()
|
||||
* or through an error in the connection (such as a remote disconnect)
|
||||
*/
|
||||
void onDisconnect(const TransportPtr& transport);
|
||||
|
||||
|
||||
void onHeaderWritten(const ConnectionPtr& conn);
|
||||
void onErrorHeaderWritten(const ConnectionPtr& conn);
|
||||
void onHeaderLengthRead(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
void onHeaderRead(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
|
||||
/**
|
||||
* \brief Read data off our transport. Also manages calling the read callback. If there is any data to be read,
|
||||
* read() will read it until the fixed read buffer is filled.
|
||||
*/
|
||||
void readTransport();
|
||||
/**
|
||||
* \brief Write data to our transport. Also manages calling the write callback.
|
||||
*/
|
||||
void writeTransport();
|
||||
|
||||
/// Are we a server? Servers wait for clients to send a header and then send a header in response.
|
||||
bool is_server_;
|
||||
/// Have we dropped?
|
||||
bool dropped_;
|
||||
/// Incoming header
|
||||
Header header_;
|
||||
/// Transport associated with us
|
||||
TransportPtr transport_;
|
||||
/// Function that handles the incoming header
|
||||
HeaderReceivedFunc header_func_;
|
||||
|
||||
/// Read buffer that ends up being passed to the read callback
|
||||
boost::shared_array<uint8_t> read_buffer_;
|
||||
/// Amount of data currently in the read buffer, in bytes
|
||||
uint32_t read_filled_;
|
||||
/// Size of the read buffer, in bytes
|
||||
uint32_t read_size_;
|
||||
/// Function to call when the read is finished
|
||||
ReadFinishedFunc read_callback_;
|
||||
/// Mutex used for protecting reading. Recursive because a read can immediately cause another read through the callback.
|
||||
boost::recursive_mutex read_mutex_;
|
||||
/// Flag telling us if we're in the middle of a read (mostly to avoid recursive deadlocking)
|
||||
bool reading_;
|
||||
/// flag telling us if there is a read callback
|
||||
/// 32-bit loads and stores are atomic on x86 and PPC... TODO: use a cross-platform atomic operations library
|
||||
/// to ensure this is done atomically
|
||||
volatile uint32_t has_read_callback_;
|
||||
|
||||
/// Buffer to write from
|
||||
boost::shared_array<uint8_t> write_buffer_;
|
||||
/// Amount of data we've written from the write buffer
|
||||
uint32_t write_sent_;
|
||||
/// Size of the write buffer
|
||||
uint32_t write_size_;
|
||||
/// Function to call when the current write is finished
|
||||
WriteFinishedFunc write_callback_;
|
||||
boost::mutex write_callback_mutex_;
|
||||
/// Mutex used for protecting writing. Recursive because a write can immediately cause another write through the callback
|
||||
boost::recursive_mutex write_mutex_;
|
||||
/// Flag telling us if we're in the middle of a write (mostly used to avoid recursive deadlocking)
|
||||
bool writing_;
|
||||
/// flag telling us if there is a write callback
|
||||
/// 32-bit loads and stores are atomic on x86 and PPC... TODO: use a cross-platform atomic operations library
|
||||
/// to ensure this is done atomically
|
||||
volatile uint32_t has_write_callback_;
|
||||
|
||||
/// Function to call when the outgoing header has finished writing
|
||||
WriteFinishedFunc header_written_callback_;
|
||||
|
||||
/// Signal raised when this connection is dropped
|
||||
DropSignal drop_signal_;
|
||||
|
||||
/// Synchronizes drop() calls
|
||||
boost::recursive_mutex drop_mutex_;
|
||||
|
||||
/// If we're sending a header error we disable most other calls
|
||||
bool sending_header_error_;
|
||||
};
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_CONNECTION_H
|
||||
106
thirdparty/ros/ros_comm/clients/roscpp/include/ros/connection_manager.h
vendored
Normal file
106
thirdparty/ros/ros_comm/clients/roscpp/include/ros/connection_manager.h
vendored
Normal file
@@ -0,0 +1,106 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#include "forwards.h"
|
||||
#include "connection.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/signals2/connection.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class PollManager;
|
||||
typedef boost::shared_ptr<PollManager> PollManagerPtr;
|
||||
|
||||
class ConnectionManager;
|
||||
typedef boost::shared_ptr<ConnectionManager> ConnectionManagerPtr;
|
||||
|
||||
class ROSCPP_DECL ConnectionManager
|
||||
{
|
||||
public:
|
||||
static const ConnectionManagerPtr& instance();
|
||||
|
||||
ConnectionManager();
|
||||
~ConnectionManager();
|
||||
|
||||
/** @brief Get a new connection ID
|
||||
*/
|
||||
uint32_t getNewConnectionID();
|
||||
|
||||
/** @brief Add a connection to be tracked by the node. Will automatically remove them if they've been dropped, but from inside the ros thread
|
||||
*
|
||||
* @param The connection to add
|
||||
*/
|
||||
void addConnection(const ConnectionPtr& connection);
|
||||
|
||||
void clear(Connection::DropReason reason);
|
||||
|
||||
uint32_t getTCPPort();
|
||||
uint32_t getUDPPort();
|
||||
|
||||
const TransportTCPPtr& getTCPServerTransport() { return tcpserver_transport_; }
|
||||
const TransportUDPPtr& getUDPServerTransport() { return udpserver_transport_; }
|
||||
|
||||
void udprosIncomingConnection(const TransportUDPPtr& transport, Header& header);
|
||||
|
||||
void start();
|
||||
void shutdown();
|
||||
|
||||
private:
|
||||
void onConnectionDropped(const ConnectionPtr& conn);
|
||||
// Remove any dropped connections from our list, causing them to be destroyed
|
||||
// They can't just be removed immediately when they're dropped because the ros
|
||||
// thread may still be using them (or more likely their transport)
|
||||
void removeDroppedConnections();
|
||||
|
||||
bool onConnectionHeaderReceived(const ConnectionPtr& conn, const Header& header);
|
||||
void tcprosAcceptConnection(const TransportTCPPtr& transport);
|
||||
|
||||
PollManagerPtr poll_manager_;
|
||||
|
||||
S_Connection connections_;
|
||||
V_Connection dropped_connections_;
|
||||
boost::mutex connections_mutex_;
|
||||
boost::mutex dropped_connections_mutex_;
|
||||
|
||||
// The connection ID counter, used to assign unique ID to each inbound or
|
||||
// outbound connection. Access via getNewConnectionID()
|
||||
uint32_t connection_id_counter_;
|
||||
boost::mutex connection_id_counter_mutex_;
|
||||
|
||||
boost::signals2::connection poll_conn_;
|
||||
|
||||
TransportTCPPtr tcpserver_transport_;
|
||||
TransportUDPPtr udpserver_transport_;
|
||||
|
||||
const static int MAX_TCPROS_CONN_QUEUE = 100; // magic
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
96
thirdparty/ros/ros_comm/clients/roscpp/include/ros/exceptions.h
vendored
Normal file
96
thirdparty/ros/ros_comm/clients/roscpp/include/ros/exceptions.h
vendored
Normal file
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_EXCEPTIONS_H
|
||||
#define ROSCPP_EXCEPTIONS_H
|
||||
|
||||
#include <ros/exception.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Thrown when an invalid node name is specified to ros::init()
|
||||
*/
|
||||
class InvalidNodeNameException : public ros::Exception
|
||||
{
|
||||
public:
|
||||
InvalidNodeNameException(const std::string& name, const std::string& reason)
|
||||
: Exception("Invalid node name [" + name + "]: " + reason)
|
||||
{}
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Thrown when an invalid graph resource name is specified to any roscpp
|
||||
* function.
|
||||
*/
|
||||
class InvalidNameException : public ros::Exception
|
||||
{
|
||||
public:
|
||||
InvalidNameException(const std::string& msg)
|
||||
: Exception(msg)
|
||||
{}
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Thrown when a second (third,...) subscription is attempted with conflicting
|
||||
* arguments.
|
||||
*/
|
||||
class ConflictingSubscriptionException : public ros::Exception
|
||||
{
|
||||
public:
|
||||
ConflictingSubscriptionException(const std::string& msg)
|
||||
: Exception(msg)
|
||||
{}
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Thrown when an invalid parameter is passed to a method
|
||||
*/
|
||||
class InvalidParameterException : public ros::Exception
|
||||
{
|
||||
public:
|
||||
InvalidParameterException(const std::string& msg)
|
||||
: Exception(msg)
|
||||
{}
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Thrown when an invalid port is specified
|
||||
*/
|
||||
class InvalidPortException : public ros::Exception
|
||||
{
|
||||
public:
|
||||
InvalidPortException(const std::string& msg)
|
||||
: Exception(msg)
|
||||
{}
|
||||
};
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif
|
||||
|
||||
52
thirdparty/ros/ros_comm/clients/roscpp/include/ros/file_log.h
vendored
Normal file
52
thirdparty/ros/ros_comm/clients/roscpp/include/ros/file_log.h
vendored
Normal file
@@ -0,0 +1,52 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_FILE_LOG_H
|
||||
#define ROSCPP_FILE_LOG_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include <ros/console.h>
|
||||
#include "common.h"
|
||||
|
||||
#define ROSCPP_LOG_DEBUG(...) ROS_DEBUG_NAMED("roscpp_internal", __VA_ARGS__)
|
||||
#define ROSCPP_CONN_LOG_DEBUG(...) ROS_DEBUG_NAMED("roscpp_internal.connections", __VA_ARGS__)
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief internal
|
||||
*/
|
||||
namespace file_log
|
||||
{
|
||||
// 20110418 TDS: this appears to be used only by rosout.
|
||||
ROSCPP_DECL const std::string& getLogDirectory();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_FILE_LOG_H
|
||||
195
thirdparty/ros/ros_comm/clients/roscpp/include/ros/forwards.h
vendored
Normal file
195
thirdparty/ros/ros_comm/clients/roscpp/include/ros/forwards.h
vendored
Normal file
@@ -0,0 +1,195 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_FORWARDS_H
|
||||
#define ROSCPP_FORWARDS_H
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <list>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/weak_ptr.hpp>
|
||||
#include <boost/function.hpp>
|
||||
|
||||
#include <ros/time.h>
|
||||
#include <ros/macros.h>
|
||||
#include "exceptions.h"
|
||||
#include "ros/datatypes.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
typedef boost::shared_ptr<void> VoidPtr;
|
||||
typedef boost::weak_ptr<void> VoidWPtr;
|
||||
typedef boost::shared_ptr<void const> VoidConstPtr;
|
||||
typedef boost::weak_ptr<void const> VoidConstWPtr;
|
||||
|
||||
class Header;
|
||||
class Transport;
|
||||
typedef boost::shared_ptr<Transport> TransportPtr;
|
||||
class TransportTCP;
|
||||
typedef boost::shared_ptr<TransportTCP> TransportTCPPtr;
|
||||
class TransportUDP;
|
||||
typedef boost::shared_ptr<TransportUDP> TransportUDPPtr;
|
||||
class Connection;
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
typedef std::set<ConnectionPtr> S_Connection;
|
||||
typedef std::vector<ConnectionPtr> V_Connection;
|
||||
class Publication;
|
||||
typedef boost::shared_ptr<Publication> PublicationPtr;
|
||||
typedef std::vector<PublicationPtr> V_Publication;
|
||||
class SubscriberLink;
|
||||
typedef boost::shared_ptr<SubscriberLink> SubscriberLinkPtr;
|
||||
typedef std::vector<SubscriberLinkPtr> V_SubscriberLink;
|
||||
class Subscription;
|
||||
typedef boost::shared_ptr<Subscription> SubscriptionPtr;
|
||||
typedef boost::weak_ptr<Subscription> SubscriptionWPtr;
|
||||
typedef std::list<SubscriptionPtr> L_Subscription;
|
||||
typedef std::vector<SubscriptionPtr> V_Subscription;
|
||||
typedef std::set<SubscriptionPtr> S_Subscription;
|
||||
class PublisherLink;
|
||||
typedef boost::shared_ptr<PublisherLink> PublisherLinkPtr;
|
||||
typedef std::vector<PublisherLinkPtr> V_PublisherLink;
|
||||
class ServicePublication;
|
||||
typedef boost::shared_ptr<ServicePublication> ServicePublicationPtr;
|
||||
typedef std::list<ServicePublicationPtr> L_ServicePublication;
|
||||
typedef std::vector<ServicePublicationPtr> V_ServicePublication;
|
||||
class ServiceServerLink;
|
||||
typedef boost::shared_ptr<ServiceServerLink> ServiceServerLinkPtr;
|
||||
typedef std::list<ServiceServerLinkPtr> L_ServiceServerLink;
|
||||
class Transport;
|
||||
typedef boost::shared_ptr<Transport> TransportPtr;
|
||||
class NodeHandle;
|
||||
typedef boost::shared_ptr<NodeHandle> NodeHandlePtr;
|
||||
|
||||
|
||||
class SingleSubscriberPublisher;
|
||||
typedef boost::function<void(const SingleSubscriberPublisher&)> SubscriberStatusCallback;
|
||||
|
||||
class CallbackQueue;
|
||||
class CallbackQueueInterface;
|
||||
class CallbackInterface;
|
||||
typedef boost::shared_ptr<CallbackInterface> CallbackInterfacePtr;
|
||||
|
||||
struct SubscriberCallbacks
|
||||
{
|
||||
SubscriberCallbacks(const SubscriberStatusCallback& connect = SubscriberStatusCallback(),
|
||||
const SubscriberStatusCallback& disconnect = SubscriberStatusCallback(),
|
||||
const VoidConstPtr& tracked_object = VoidConstPtr(),
|
||||
CallbackQueueInterface* callback_queue = 0)
|
||||
: connect_(connect)
|
||||
, disconnect_(disconnect)
|
||||
, callback_queue_(callback_queue)
|
||||
{
|
||||
has_tracked_object_ = false;
|
||||
if (tracked_object)
|
||||
{
|
||||
has_tracked_object_ = true;
|
||||
tracked_object_ = tracked_object;
|
||||
}
|
||||
}
|
||||
SubscriberStatusCallback connect_;
|
||||
SubscriberStatusCallback disconnect_;
|
||||
|
||||
bool has_tracked_object_;
|
||||
VoidConstWPtr tracked_object_;
|
||||
CallbackQueueInterface* callback_queue_;
|
||||
};
|
||||
typedef boost::shared_ptr<SubscriberCallbacks> SubscriberCallbacksPtr;
|
||||
|
||||
/**
|
||||
* \brief Structure passed as a parameter to the callback invoked by a ros::Timer
|
||||
*/
|
||||
struct TimerEvent
|
||||
{
|
||||
Time last_expected; ///< In a perfect world, this is when the last callback should have happened
|
||||
Time last_real; ///< When the last callback actually happened
|
||||
|
||||
Time current_expected; ///< In a perfect world, this is when the current callback should be happening
|
||||
Time current_real; ///< This is when the current callback was actually called (Time::now() as of the beginning of the callback)
|
||||
|
||||
struct
|
||||
{
|
||||
WallDuration last_duration; ///< How long the last callback ran for
|
||||
} profile;
|
||||
};
|
||||
typedef boost::function<void(const TimerEvent&)> TimerCallback;
|
||||
|
||||
/**
|
||||
* \brief Structure passed as a parameter to the callback invoked by a ros::WallTimer
|
||||
*/
|
||||
struct WallTimerEvent
|
||||
{
|
||||
WallTime last_expected; ///< In a perfect world, this is when the last callback should have happened
|
||||
WallTime last_real; ///< When the last callback actually happened
|
||||
|
||||
WallTime current_expected; ///< In a perfect world, this is when the current callback should be happening
|
||||
WallTime current_real; ///< This is when the current callback was actually called (Time::now() as of the beginning of the callback)
|
||||
|
||||
struct
|
||||
{
|
||||
WallDuration last_duration; ///< How long the last callback ran for
|
||||
} profile;
|
||||
};
|
||||
typedef boost::function<void(const WallTimerEvent&)> WallTimerCallback;
|
||||
|
||||
/**
|
||||
* \brief Structure passed as a parameter to the callback invoked by a ros::SteadyTimer
|
||||
*/
|
||||
struct SteadyTimerEvent
|
||||
{
|
||||
SteadyTime last_expected; ///< In a perfect world, this is when the last callback should have happened
|
||||
SteadyTime last_real; ///< When the last callback actually happened
|
||||
|
||||
SteadyTime current_expected; ///< In a perfect world, this is when the current callback should be happening
|
||||
SteadyTime current_real; ///< This is when the current callback was actually called (SteadyTime::now() as of the beginning of the callback)
|
||||
|
||||
struct
|
||||
{
|
||||
WallDuration last_duration; ///< How long the last callback ran for
|
||||
} profile;
|
||||
};
|
||||
typedef boost::function<void(const SteadyTimerEvent&)> SteadyTimerCallback;
|
||||
|
||||
class ServiceManager;
|
||||
typedef boost::shared_ptr<ServiceManager> ServiceManagerPtr;
|
||||
class TopicManager;
|
||||
typedef boost::shared_ptr<TopicManager> TopicManagerPtr;
|
||||
class ConnectionManager;
|
||||
typedef boost::shared_ptr<ConnectionManager> ConnectionManagerPtr;
|
||||
class XMLRPCManager;
|
||||
typedef boost::shared_ptr<XMLRPCManager> XMLRPCManagerPtr;
|
||||
class PollManager;
|
||||
typedef boost::shared_ptr<PollManager> PollManagerPtr;
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
220
thirdparty/ros/ros_comm/clients/roscpp/include/ros/init.h
vendored
Normal file
220
thirdparty/ros/ros_comm/clients/roscpp/include/ros/init.h
vendored
Normal file
@@ -0,0 +1,220 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_INIT_H
|
||||
#define ROSCPP_INIT_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/spinner.h"
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
namespace init_options
|
||||
{
|
||||
/**
|
||||
* \brief Flags for ROS initialization
|
||||
*/
|
||||
enum InitOption
|
||||
{
|
||||
/**
|
||||
* Don't install a SIGINT handler. You should install your own SIGINT handler in this
|
||||
* case, to ensure that the node gets shutdown correctly when it exits.
|
||||
*/
|
||||
NoSigintHandler = 1 << 0,
|
||||
/** \brief Anonymize the node name. Adds a random number to the end of your node's name, to make it unique.
|
||||
*/
|
||||
AnonymousName = 1 << 1,
|
||||
/**
|
||||
* \brief Don't broadcast rosconsole output to the /rosout topic
|
||||
*/
|
||||
NoRosout = 1 << 2,
|
||||
};
|
||||
}
|
||||
typedef init_options::InitOption InitOption;
|
||||
|
||||
/** @brief ROS initialization function.
|
||||
*
|
||||
* This function will parse any ROS arguments (e.g., topic name
|
||||
* remappings), and will consume them (i.e., argc and argv may be modified
|
||||
* as a result of this call).
|
||||
*
|
||||
* Use this version if you are using the NodeHandle API
|
||||
*
|
||||
* \param argc
|
||||
* \param argv
|
||||
* \param name Name of this node. The name must be a base name, ie. it cannot contain namespaces.
|
||||
* \param options [optional] Options to start the node with (a set of bit flags from \ref ros::init_options)
|
||||
* \throws InvalidNodeNameException If the name passed in is not a valid "base" name
|
||||
*
|
||||
*/
|
||||
ROSCPP_DECL void init(int &argc, char **argv, const std::string& name, uint32_t options = 0);
|
||||
|
||||
/**
|
||||
* \brief alternate ROS initialization function.
|
||||
*
|
||||
* \param remappings A map<string, string> where each one constitutes a name remapping, or one of the special remappings like __name, __master, __ns, etc.
|
||||
* \param name Name of this node. The name must be a base name, ie. it cannot contain namespaces.
|
||||
* \param options [optional] Options to start the node with (a set of bit flags from \ref ros::init_options)
|
||||
* \throws InvalidNodeNameException If the name passed in is not a valid "base" name
|
||||
*/
|
||||
ROSCPP_DECL void init(const M_string& remappings, const std::string& name, uint32_t options = 0);
|
||||
|
||||
/**
|
||||
* \brief alternate ROS initialization function.
|
||||
*
|
||||
* \param remappings A vector<pair<string, string> > where each one constitutes a name remapping, or one of the special remappings like __name, __master, __ns, etc.
|
||||
* \param name Name of this node. The name must be a base name, ie. it cannot contain namespaces.
|
||||
* \param options [optional] Options to start the node with (a set of bit flags from \ref ros::init_options)
|
||||
* \throws InvalidNodeNameException If the name passed in is not a valid "base" name
|
||||
*/
|
||||
ROSCPP_DECL void init(const VP_string& remapping_args, const std::string& name, uint32_t options = 0);
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not ros::init() has been called
|
||||
*/
|
||||
ROSCPP_DECL bool isInitialized();
|
||||
/**
|
||||
* \brief Returns whether or not ros::shutdown() has been (or is being) called
|
||||
*/
|
||||
ROSCPP_DECL bool isShuttingDown();
|
||||
|
||||
/** \brief Enter simple event loop
|
||||
*
|
||||
* This method enters a loop, processing callbacks. This method should only be used
|
||||
* if the NodeHandle API is being used.
|
||||
*
|
||||
* This method is mostly useful when your node does all of its work in
|
||||
* subscription callbacks. It will not process any callbacks that have been assigned to
|
||||
* custom queues.
|
||||
*
|
||||
*/
|
||||
ROSCPP_DECL void spin();
|
||||
|
||||
/** \brief Enter simple event loop
|
||||
*
|
||||
* This method enters a loop, processing callbacks. This method should only be used
|
||||
* if the NodeHandle API is being used.
|
||||
*
|
||||
* This method is mostly useful when your node does all of its work in
|
||||
* subscription callbacks. It will not process any callbacks that have been assigned to
|
||||
* custom queues.
|
||||
*
|
||||
* \param spinner a spinner to use to call callbacks. Two default implementations are available,
|
||||
* SingleThreadedSpinner and MultiThreadedSpinner
|
||||
*/
|
||||
ROSCPP_DECL void spin(Spinner& spinner);
|
||||
/**
|
||||
* \brief Process a single round of callbacks.
|
||||
*
|
||||
* This method is useful if you have your own loop running and would like to process
|
||||
* any callbacks that are available. This is equivalent to calling callAvailable() on the
|
||||
* global CallbackQueue. It will not process any callbacks that have been assigned to
|
||||
* custom queues.
|
||||
*/
|
||||
ROSCPP_DECL void spinOnce();
|
||||
|
||||
/**
|
||||
* \brief Wait for this node to be shutdown, whether through Ctrl-C, ros::shutdown(), or similar.
|
||||
*/
|
||||
ROSCPP_DECL void waitForShutdown();
|
||||
|
||||
/** \brief Check whether it's time to exit.
|
||||
*
|
||||
* ok() becomes false once ros::shutdown() has been called and is finished
|
||||
*
|
||||
* \return true if we're still OK, false if it's time to exit
|
||||
*/
|
||||
ROSCPP_DECL bool ok();
|
||||
/**
|
||||
* \brief Disconnects everything and unregisters from the master. It is generally not
|
||||
* necessary to call this function, as the node will automatically shutdown when all
|
||||
* NodeHandles destruct. However, if you want to break out of a spin() loop explicitly,
|
||||
* this function allows that.
|
||||
*/
|
||||
ROSCPP_DECL void shutdown();
|
||||
|
||||
/**
|
||||
* \brief Request that the node shut itself down from within a ROS thread
|
||||
*
|
||||
* This method signals a ROS thread to call shutdown().
|
||||
*/
|
||||
ROSCPP_DECL void requestShutdown();
|
||||
|
||||
/**
|
||||
* \brief Actually starts the internals of the node (spins up threads, starts the network polling and xmlrpc loops,
|
||||
* connects to internal subscriptions like /clock, starts internal service servers, etc.).
|
||||
*
|
||||
* Usually unnecessary to call manually, as it is automatically called by the creation of the first NodeHandle if
|
||||
* the node has not already been started. If you would like to prevent the automatic shutdown caused by the last
|
||||
* NodeHandle going out of scope, call this before any NodeHandle has been created (e.g. immediately after init())
|
||||
*/
|
||||
ROSCPP_DECL void start();
|
||||
/**
|
||||
* \brief Returns whether or not the node has been started through ros::start()
|
||||
*/
|
||||
ROSCPP_DECL bool isStarted();
|
||||
|
||||
/**
|
||||
* \brief Returns a pointer to the global default callback queue.
|
||||
*
|
||||
* This is the queue that all callbacks get added to unless a different one is specified, either in the NodeHandle
|
||||
* or in the individual NodeHandle::subscribe()/NodeHandle::advertise()/etc. functions.
|
||||
*/
|
||||
ROSCPP_DECL CallbackQueue* getGlobalCallbackQueue();
|
||||
|
||||
/**
|
||||
* \brief searches the command line arguments for the given arg parameter. In case this argument is not found
|
||||
* an empty string is returned.
|
||||
*
|
||||
* \param argc the number of command-line arguments
|
||||
* \param argv the command-line arguments
|
||||
* \param arg argument to search for
|
||||
*/
|
||||
ROSCPP_DECL std::string getROSArg(int argc, const char* const* argv, const std::string& arg);
|
||||
|
||||
/**
|
||||
* \brief returns a vector of program arguments that do not include any ROS remapping arguments. Useful if you need
|
||||
* to parse your arguments to determine your node name
|
||||
*
|
||||
* \param argc the number of command-line arguments
|
||||
* \param argv the command-line arguments
|
||||
* \param [out] args_out Output args, stripped of any ROS args
|
||||
*/
|
||||
ROSCPP_DECL void removeROSArgs(int argc, const char* const* argv, V_string& args_out);
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
47
thirdparty/ros/ros_comm/clients/roscpp/include/ros/internal_timer_manager.h
vendored
Normal file
47
thirdparty/ros/ros_comm/clients/roscpp/include/ros/internal_timer_manager.h
vendored
Normal file
@@ -0,0 +1,47 @@
|
||||
/*
|
||||
* Copyright (C) 2010, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_INTERNAL_TIMER_MANAGER_H
|
||||
#define ROSCPP_INTERNAL_TIMER_MANAGER_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include <ros/time.h>
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
template<typename T, typename D, typename E> class TimerManager;
|
||||
typedef TimerManager<SteadyTime, WallDuration, SteadyTimerEvent> InternalTimerManager;
|
||||
typedef boost::shared_ptr<InternalTimerManager> InternalTimerManagerPtr;
|
||||
|
||||
ROSCPP_DECL void initInternalTimerManager();
|
||||
ROSCPP_DECL InternalTimerManagerPtr getInternalTimerManager();
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_INTERNAL_TIMER_MANAGER_H
|
||||
80
thirdparty/ros/ros_comm/clients/roscpp/include/ros/intraprocess_publisher_link.h
vendored
Normal file
80
thirdparty/ros/ros_comm/clients/roscpp/include/ros/intraprocess_publisher_link.h
vendored
Normal file
@@ -0,0 +1,80 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_INTRAPROCESS_PUBLISHER_LINK_H
|
||||
#define ROSCPP_INTRAPROCESS_PUBLISHER_LINK_H
|
||||
|
||||
#include "publisher_link.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
class Subscription;
|
||||
typedef boost::shared_ptr<Subscription> SubscriptionPtr;
|
||||
typedef boost::weak_ptr<Subscription> SubscriptionWPtr;
|
||||
|
||||
class IntraProcessSubscriberLink;
|
||||
typedef boost::shared_ptr<IntraProcessSubscriberLink> IntraProcessSubscriberLinkPtr;
|
||||
|
||||
/**
|
||||
* \brief Handles a connection to a single publisher on a given topic. Receives messages from a publisher
|
||||
* and hands them off to its parent Subscription
|
||||
*/
|
||||
class ROSCPP_DECL IntraProcessPublisherLink : public PublisherLink
|
||||
{
|
||||
public:
|
||||
IntraProcessPublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri, const TransportHints& transport_hints);
|
||||
virtual ~IntraProcessPublisherLink();
|
||||
|
||||
void setPublisher(const IntraProcessSubscriberLinkPtr& publisher);
|
||||
|
||||
virtual std::string getTransportType();
|
||||
virtual std::string getTransportInfo();
|
||||
virtual void drop();
|
||||
|
||||
/**
|
||||
* \brief Handles handing off a received message to the subscription, where it will be deserialized and called back
|
||||
*/
|
||||
virtual void handleMessage(const SerializedMessage& m, bool ser, bool nocopy);
|
||||
|
||||
void getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti);
|
||||
|
||||
private:
|
||||
IntraProcessSubscriberLinkPtr publisher_;
|
||||
bool dropped_;
|
||||
boost::recursive_mutex drop_mutex_;
|
||||
};
|
||||
typedef boost::shared_ptr<IntraProcessPublisherLink> IntraProcessPublisherLinkPtr;
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_INTRAPROCESS_PUBLISHER_LINK_H
|
||||
|
||||
|
||||
|
||||
69
thirdparty/ros/ros_comm/clients/roscpp/include/ros/intraprocess_subscriber_link.h
vendored
Normal file
69
thirdparty/ros/ros_comm/clients/roscpp/include/ros/intraprocess_subscriber_link.h
vendored
Normal file
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_INTRAPROCESS_SUBSCRIBER_LINK_H
|
||||
#define ROSCPP_INTRAPROCESS_SUBSCRIBER_LINK_H
|
||||
#include "subscriber_link.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class IntraProcessPublisherLink;
|
||||
typedef boost::shared_ptr<IntraProcessPublisherLink> IntraProcessPublisherLinkPtr;
|
||||
|
||||
/**
|
||||
* \brief SubscriberLink handles broadcasting messages to a single subscriber on a single topic
|
||||
*/
|
||||
class ROSCPP_DECL IntraProcessSubscriberLink : public SubscriberLink
|
||||
{
|
||||
public:
|
||||
IntraProcessSubscriberLink(const PublicationPtr& parent);
|
||||
virtual ~IntraProcessSubscriberLink();
|
||||
|
||||
void setSubscriber(const IntraProcessPublisherLinkPtr& subscriber);
|
||||
bool isLatching();
|
||||
|
||||
virtual void enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy);
|
||||
virtual void drop();
|
||||
virtual std::string getTransportType();
|
||||
virtual std::string getTransportInfo();
|
||||
virtual bool isIntraprocess() { return true; }
|
||||
virtual void getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti);
|
||||
|
||||
private:
|
||||
IntraProcessPublisherLinkPtr subscriber_;
|
||||
bool dropped_;
|
||||
boost::recursive_mutex drop_mutex_;
|
||||
};
|
||||
typedef boost::shared_ptr<IntraProcessSubscriberLink> IntraProcessSubscriberLinkPtr;
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_INTRAPROCESS_SUBSCRIBER_LINK_H
|
||||
216
thirdparty/ros/ros_comm/clients/roscpp/include/ros/io.h
vendored
Normal file
216
thirdparty/ros/ros_comm/clients/roscpp/include/ros/io.h
vendored
Normal file
@@ -0,0 +1,216 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
/*****************************************************************************
|
||||
** Ifdefs
|
||||
*****************************************************************************/
|
||||
#ifndef ROSCPP_IO_H_
|
||||
#define ROSCPP_IO_H_
|
||||
|
||||
/*****************************************************************************
|
||||
** Includes
|
||||
*****************************************************************************/
|
||||
|
||||
#include <string>
|
||||
#include "common.h"
|
||||
|
||||
#ifdef WIN32
|
||||
#include <winsock2.h> // For struct timeval
|
||||
#include <ws2tcpip.h> // Must be after winsock2.h because MS didn't put proper inclusion guards in their headers.
|
||||
#include <sys/types.h>
|
||||
#include <io.h>
|
||||
#include <fcntl.h>
|
||||
#include <process.h> // for _getpid
|
||||
#else
|
||||
#include <poll.h> // should get cmake to explicitly check for poll.h?
|
||||
#include <sys/poll.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <netdb.h>
|
||||
#include <unistd.h>
|
||||
#include <netdb.h> // getnameinfo in network.cpp
|
||||
#include <netinet/in.h> // sockaddr_in in network.cpp
|
||||
#include <netinet/tcp.h> // TCP_NODELAY in transport/transport_tcp.cpp
|
||||
#endif
|
||||
|
||||
/*****************************************************************************
|
||||
** Cross Platform Macros
|
||||
*****************************************************************************/
|
||||
|
||||
#ifdef WIN32
|
||||
#define getpid _getpid
|
||||
#define ROS_INVALID_SOCKET INVALID_SOCKET
|
||||
#define ROS_SOCKETS_SHUT_RDWR SD_BOTH /* Used by ::shutdown() */
|
||||
#define ROS_SOCKETS_ASYNCHRONOUS_CONNECT_RETURN WSAEWOULDBLOCK
|
||||
#ifndef POLLRDNORM
|
||||
#define POLLRDNORM 0x0100 /* mapped to read fds_set */
|
||||
#endif
|
||||
#ifndef POLLRDBAND
|
||||
#define POLLRDBAND 0x0200 /* mapped to exception fds_set */
|
||||
#endif
|
||||
#ifndef POLLIN
|
||||
#define POLLIN (POLLRDNORM | POLLRDBAND) /* There is data to read. */
|
||||
#endif
|
||||
#ifndef POLLPRI
|
||||
#define POLLPRI 0x0400 /* There is urgent data to read. */
|
||||
#endif
|
||||
|
||||
#ifndef POLLWRNORM
|
||||
#define POLLWRNORM 0x0010 /* mapped to write fds_set */
|
||||
#endif
|
||||
#ifndef POLLOUT
|
||||
#define POLLOUT (POLLWRNORM) /* Writing now will not block. */
|
||||
#endif
|
||||
#ifndef POLLWRBAND
|
||||
#define POLLWRBAND 0x0020 /* mapped to write fds_set */
|
||||
#endif
|
||||
#ifndef POLLERR
|
||||
#define POLLERR 0x0001 /* Error condition. */
|
||||
#endif
|
||||
#ifndef POLLHUP
|
||||
#define POLLHUP 0x0002 /* Hung up. */
|
||||
#endif
|
||||
#ifndef POLLNVAL
|
||||
#define POLLNVAL 0x0004 /* Invalid polling request. */
|
||||
#endif
|
||||
#else
|
||||
#define ROS_SOCKETS_SHUT_RDWR SHUT_RDWR /* Used by ::shutdown() */
|
||||
#define ROS_INVALID_SOCKET ((int) -1)
|
||||
#define ROS_SOCKETS_ASYNCHRONOUS_CONNECT_RETURN EINPROGRESS
|
||||
#endif
|
||||
|
||||
/*****************************************************************************
|
||||
** Namespaces
|
||||
*****************************************************************************/
|
||||
|
||||
namespace ros {
|
||||
|
||||
/*****************************************************************************
|
||||
** Cross Platform Types
|
||||
*****************************************************************************/
|
||||
|
||||
#ifdef WIN32
|
||||
typedef SOCKET socket_fd_t;
|
||||
typedef SOCKET signal_fd_t;
|
||||
/* poll emulation support */
|
||||
typedef struct socket_pollfd {
|
||||
socket_fd_t fd; /* file descriptor */
|
||||
short events; /* requested events */
|
||||
short revents; /* returned events */
|
||||
} socket_pollfd;
|
||||
|
||||
typedef unsigned long int nfds_t;
|
||||
#ifdef _MSC_VER
|
||||
typedef int pid_t; /* return type for windows' _getpid */
|
||||
#endif
|
||||
#else
|
||||
typedef int socket_fd_t;
|
||||
typedef int signal_fd_t;
|
||||
typedef struct pollfd socket_pollfd;
|
||||
#endif
|
||||
|
||||
typedef boost::shared_ptr<std::vector<socket_pollfd> > pollfd_vector_ptr;
|
||||
|
||||
/*****************************************************************************
|
||||
** Functions
|
||||
*****************************************************************************/
|
||||
|
||||
ROSCPP_DECL int last_socket_error();
|
||||
ROSCPP_DECL const char* last_socket_error_string();
|
||||
ROSCPP_DECL bool last_socket_error_is_would_block();
|
||||
ROSCPP_DECL pollfd_vector_ptr poll_sockets(int epfd, socket_pollfd *fds, nfds_t nfds, int timeout);
|
||||
ROSCPP_DECL int set_non_blocking(socket_fd_t &socket);
|
||||
ROSCPP_DECL int close_socket(socket_fd_t &socket);
|
||||
ROSCPP_DECL int create_signal_pair(signal_fd_t signal_pair[2]);
|
||||
|
||||
ROSCPP_DECL int create_socket_watcher();
|
||||
ROSCPP_DECL void close_socket_watcher(int fd);
|
||||
ROSCPP_DECL void add_socket_to_watcher(int epfd, int fd);
|
||||
ROSCPP_DECL void del_socket_from_watcher(int epfd, int fd);
|
||||
ROSCPP_DECL void set_events_on_socket(int epfd, int fd, int events);
|
||||
|
||||
/*****************************************************************************
|
||||
** Inlines - almost direct api replacements, should stay fast.
|
||||
*****************************************************************************/
|
||||
|
||||
/**
|
||||
* Closes the signal pair - on windows we're using sockets (because windows
|
||||
* select() function cant handle pipes). On linux, we're just using the pipes.
|
||||
* @param signal_pair : the signal pair type.
|
||||
*/
|
||||
inline void close_signal_pair(signal_fd_t signal_pair[2]) {
|
||||
#ifdef WIN32 // use a socket pair
|
||||
::closesocket(signal_pair[0]);
|
||||
::closesocket(signal_pair[1]);
|
||||
#else // use a socket pair on mingw or pipe pair on linux, either way, close works
|
||||
::close(signal_pair[0]);
|
||||
::close(signal_pair[1]);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Write to a signal_fd_t device. On windows we're using sockets (because windows
|
||||
* select() function cant handle pipes) so we have to use socket functions.
|
||||
* On linux, we're just using the pipes.
|
||||
*/
|
||||
#ifdef _MSC_VER
|
||||
inline int write_signal(const signal_fd_t &signal, const char *buffer, const unsigned int &nbyte) {
|
||||
return ::send(signal, buffer, nbyte, 0);
|
||||
// return write(signal, buffer, nbyte);
|
||||
}
|
||||
#else
|
||||
inline ssize_t write_signal(const signal_fd_t &signal, const void *buffer, const size_t &nbyte) {
|
||||
return write(signal, buffer, nbyte);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* Read from a signal_fd_t device. On windows we're using sockets (because windows
|
||||
* select() function cant handle pipes) so we have to use socket functions.
|
||||
* On linux, we're just using the pipes.
|
||||
*/
|
||||
#ifdef _MSC_VER
|
||||
inline int read_signal(const signal_fd_t &signal, char *buffer, const unsigned int &nbyte) {
|
||||
return ::recv(signal, buffer, nbyte, 0);
|
||||
// return _read(signal, buffer, nbyte);
|
||||
}
|
||||
#else
|
||||
inline ssize_t read_signal(const signal_fd_t &signal, void *buffer, const size_t &nbyte) {
|
||||
return read(signal, buffer, nbyte);
|
||||
}
|
||||
#endif
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif /* ROSCPP_IO_H_ */
|
||||
|
||||
130
thirdparty/ros/ros_comm/clients/roscpp/include/ros/master.h
vendored
Normal file
130
thirdparty/ros/ros_comm/clients/roscpp/include/ros/master.h
vendored
Normal file
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_MASTER_H
|
||||
#define ROSCPP_MASTER_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "xmlrpcpp/XmlRpcValue.h"
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Contains functions which allow you to query information about the master
|
||||
*/
|
||||
namespace master
|
||||
{
|
||||
|
||||
/** @brief Execute an XMLRPC call on the master
|
||||
*
|
||||
* @param method The RPC method to invoke
|
||||
* @param request The arguments to the RPC call
|
||||
* @param response [out] The resonse that was received.
|
||||
* @param payload [out] The payload that was received.
|
||||
* @param wait_for_master Whether or not this call should loop until it can contact the master
|
||||
*
|
||||
* @return true if call succeeds, false otherwise.
|
||||
*/
|
||||
ROSCPP_DECL bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master);
|
||||
|
||||
/** @brief Get the hostname where the master runs.
|
||||
*
|
||||
* @return The master's hostname, as a string
|
||||
*/
|
||||
ROSCPP_DECL const std::string& getHost();
|
||||
/** @brief Get the port where the master runs.
|
||||
*
|
||||
* @return The master's port.
|
||||
*/
|
||||
ROSCPP_DECL uint32_t getPort();
|
||||
/**
|
||||
* \brief Get the full URI to the master (eg. http://host:port/)
|
||||
*/
|
||||
ROSCPP_DECL const std::string& getURI();
|
||||
|
||||
/** @brief Check whether the master is up
|
||||
*
|
||||
* This method tries to contact the master. You can call it any time
|
||||
* after ros::init has been called. The intended usage is to check
|
||||
* whether the master is up before trying to make other requests
|
||||
* (subscriptions, advertisements, etc.).
|
||||
*
|
||||
* @returns true if the master is available, false otherwise.
|
||||
*/
|
||||
ROSCPP_DECL bool check();
|
||||
|
||||
/**
|
||||
* \brief Contains information retrieved from the master about a topic
|
||||
*/
|
||||
struct ROSCPP_DECL TopicInfo
|
||||
{
|
||||
TopicInfo() {}
|
||||
TopicInfo(const std::string& _name, const std::string& _datatype /*, const std::string& _md5sum*/)
|
||||
: name(_name)
|
||||
, datatype(_datatype)
|
||||
//, md5sum(_md5sum)
|
||||
{}
|
||||
std::string name; ///< Name of the topic
|
||||
std::string datatype; ///< Datatype of the topic
|
||||
|
||||
// not possible yet unfortunately (master does not have this information)
|
||||
//std::string md5sum; ///< md5sum of the topic
|
||||
};
|
||||
typedef std::vector<TopicInfo> V_TopicInfo;
|
||||
|
||||
/** @brief Get the list of topics that are being published by all nodes.
|
||||
*
|
||||
* This method communicates with the master to retrieve the list of all
|
||||
* currently advertised topics.
|
||||
*
|
||||
* @param topics A place to store the resulting list. Each item in the
|
||||
* list is a pair <string topic, string type>. The type is represented
|
||||
* in the format "package_name/MessageName", and is also retrievable
|
||||
* through message.__getDataType() or MessageName::__s_getDataType().
|
||||
*
|
||||
* @return true on success, false otherwise (topics not filled in)
|
||||
*/
|
||||
ROSCPP_DECL bool getTopics(V_TopicInfo& topics);
|
||||
|
||||
/**
|
||||
* \brief Retreives the currently-known list of nodes from the master
|
||||
*/
|
||||
ROSCPP_DECL bool getNodes(V_string& nodes);
|
||||
|
||||
/**
|
||||
* @brief Set the max time this node should spend looping trying to connect to the master
|
||||
* @param The timeout. A negative value means infinite
|
||||
*/
|
||||
ROSCPP_DECL void setRetryTimeout(ros::WallDuration timeout);
|
||||
|
||||
} // namespace master
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif
|
||||
88
thirdparty/ros/ros_comm/clients/roscpp/include/ros/message.h
vendored
Normal file
88
thirdparty/ros/ros_comm/clients/roscpp/include/ros/message.h
vendored
Normal file
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_MESSAGE_H
|
||||
#define ROSCPP_MESSAGE_H
|
||||
|
||||
// #warning You should not be using this file
|
||||
|
||||
#include "ros/macros.h"
|
||||
#include "ros/assert.h"
|
||||
#include <string>
|
||||
#include <string.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/array.hpp>
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#define ROSCPP_MESSAGE_HAS_DEFINITION
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
typedef std::map<std::string, std::string> M_string;
|
||||
|
||||
/**
|
||||
* \deprecated This base-class is deprecated in favor of a template-based serialization and traits system
|
||||
*/
|
||||
#if 0
|
||||
class Message
|
||||
{
|
||||
public:
|
||||
typedef boost::shared_ptr<Message> Ptr;
|
||||
typedef boost::shared_ptr<Message const> ConstPtr;
|
||||
Message()
|
||||
{
|
||||
}
|
||||
virtual ~Message()
|
||||
{
|
||||
}
|
||||
virtual const std::string __getDataType() const = 0;
|
||||
virtual const std::string __getMD5Sum() const = 0;
|
||||
virtual const std::string __getMessageDefinition() const = 0;
|
||||
inline static std::string __s_getDataType() { ROS_BREAK(); return std::string(""); }
|
||||
inline static std::string __s_getMD5Sum() { ROS_BREAK(); return std::string(""); }
|
||||
inline static std::string __s_getMessageDefinition() { ROS_BREAK(); return std::string(""); }
|
||||
virtual uint32_t serializationLength() const = 0;
|
||||
virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const = 0;
|
||||
virtual uint8_t *deserialize(uint8_t *read_ptr) = 0;
|
||||
uint32_t __serialized_length;
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Message> MessagePtr;
|
||||
typedef boost::shared_ptr<Message const> MessageConstPtr;
|
||||
#endif
|
||||
|
||||
#define SROS_SERIALIZE_PRIMITIVE(ptr, data) { memcpy(ptr, &data, sizeof(data)); ptr += sizeof(data); }
|
||||
#define SROS_SERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(ptr, data, data_size); ptr += data_size; } }
|
||||
#define SROS_DESERIALIZE_PRIMITIVE(ptr, data) { memcpy(&data, ptr, sizeof(data)); ptr += sizeof(data); }
|
||||
#define SROS_DESERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(data, ptr, data_size); ptr += data_size; } }
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
66
thirdparty/ros/ros_comm/clients/roscpp/include/ros/message_deserializer.h
vendored
Normal file
66
thirdparty/ros/ros_comm/clients/roscpp/include/ros/message_deserializer.h
vendored
Normal file
@@ -0,0 +1,66 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_MESSAGE_DESERIALIZER_H
|
||||
#define ROSCPP_MESSAGE_DESERIALIZER_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <ros/serialized_message.h>
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class SubscriptionCallbackHelper;
|
||||
typedef boost::shared_ptr<SubscriptionCallbackHelper> SubscriptionCallbackHelperPtr;
|
||||
|
||||
class ROSCPP_DECL MessageDeserializer
|
||||
{
|
||||
public:
|
||||
MessageDeserializer(const SubscriptionCallbackHelperPtr& helper, const SerializedMessage& m, const boost::shared_ptr<M_string>& connection_header);
|
||||
|
||||
VoidConstPtr deserialize();
|
||||
const boost::shared_ptr<M_string>& getConnectionHeader() { return connection_header_; }
|
||||
|
||||
private:
|
||||
SubscriptionCallbackHelperPtr helper_;
|
||||
SerializedMessage serialized_message_;
|
||||
boost::shared_ptr<M_string> connection_header_;
|
||||
|
||||
boost::mutex mutex_;
|
||||
VoidConstPtr msg_;
|
||||
};
|
||||
typedef boost::shared_ptr<MessageDeserializer> MessageDeserializerPtr;
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_MESSAGE_DESERIALIZER_H
|
||||
|
||||
96
thirdparty/ros/ros_comm/clients/roscpp/include/ros/names.h
vendored
Normal file
96
thirdparty/ros/ros_comm/clients/roscpp/include/ros/names.h
vendored
Normal file
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_NAMES_H
|
||||
#define ROSCPP_NAMES_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Contains functions which allow you to manipulate ROS names
|
||||
*/
|
||||
namespace names
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Cleans a graph resource name: removes double slashes, trailing slash
|
||||
*/
|
||||
ROSCPP_DECL std::string clean(const std::string& name);
|
||||
/**
|
||||
* \brief Resolve a graph resource name into a fully qualified graph resource name
|
||||
*
|
||||
* See http://www.ros.org/wiki/Names for more details
|
||||
*
|
||||
* \param name Name to resolve
|
||||
* \param remap Whether or not to apply remappings to the name
|
||||
* \throws InvalidNameException if the name passed is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL std::string resolve(const std::string& name, bool remap = true);
|
||||
/**
|
||||
* \brief Resolve a graph resource name into a fully qualified graph resource name
|
||||
*
|
||||
* See http://www.ros.org/wiki/Names for more details
|
||||
*
|
||||
* \param ns Namespace to use in resolution
|
||||
* \param name Name to resolve
|
||||
* \param remap Whether or not to apply remappings to the name
|
||||
* \throws InvalidNameException if the name passed is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL std::string resolve(const std::string& ns, const std::string& name, bool remap = true);
|
||||
/**
|
||||
* \brief Append one name to another
|
||||
*/
|
||||
ROSCPP_DECL std::string append(const std::string& left, const std::string& right);
|
||||
/**
|
||||
* \brief Apply remappings to a name
|
||||
* \throws InvalidNameException if the name passed is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL std::string remap(const std::string& name);
|
||||
/**
|
||||
* \brief Validate a name against the name spec
|
||||
*/
|
||||
ROSCPP_DECL bool validate(const std::string& name, std::string& error);
|
||||
|
||||
ROSCPP_DECL const M_string& getRemappings();
|
||||
ROSCPP_DECL const M_string& getUnresolvedRemappings();
|
||||
|
||||
/**
|
||||
* \brief Get the parent namespace of a name
|
||||
* \param name The namespace of which to get the parent namespace.
|
||||
* \throws InvalidNameException if the name passed is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL std::string parentNamespace(const std::string& name);
|
||||
|
||||
} // namespace names
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_NAMES_H
|
||||
51
thirdparty/ros/ros_comm/clients/roscpp/include/ros/network.h
vendored
Normal file
51
thirdparty/ros/ros_comm/clients/roscpp/include/ros/network.h
vendored
Normal file
@@ -0,0 +1,51 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_NETWORK_H
|
||||
#define ROSCPP_NETWORK_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief internal
|
||||
*/
|
||||
namespace network
|
||||
{
|
||||
|
||||
ROSCPP_DECL bool splitURI(const std::string& uri, std::string& host, uint32_t& port);
|
||||
ROSCPP_DECL const std::string& getHost();
|
||||
ROSCPP_DECL uint16_t getTCPROSPort();
|
||||
|
||||
} // namespace network
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif
|
||||
2203
thirdparty/ros/ros_comm/clients/roscpp/include/ros/node_handle.h
vendored
Normal file
2203
thirdparty/ros/ros_comm/clients/roscpp/include/ros/node_handle.h
vendored
Normal file
File diff suppressed because it is too large
Load Diff
651
thirdparty/ros/ros_comm/clients/roscpp/include/ros/param.h
vendored
Normal file
651
thirdparty/ros/ros_comm/clients/roscpp/include/ros/param.h
vendored
Normal file
@@ -0,0 +1,651 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_PARAM_H
|
||||
#define ROSCPP_PARAM_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "common.h"
|
||||
#include "xmlrpcpp/XmlRpcValue.h"
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Contains functions which allow you to query the parameter server
|
||||
*/
|
||||
namespace param
|
||||
{
|
||||
|
||||
/** \brief Set an arbitrary XML/RPC value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param v The value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const XmlRpc::XmlRpcValue& v);
|
||||
/** \brief Set a string value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param s The value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::string& s);
|
||||
/** \brief Set a string value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param s The value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const char* s);
|
||||
/** \brief Set a double value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param d The value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, double d);
|
||||
/** \brief Set an integer value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param i The value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, int i);
|
||||
/** \brief Set a bool value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param b The value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, bool b);
|
||||
|
||||
|
||||
/** \brief Set a string vector value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param vec The vector value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::vector<std::string>& vec);
|
||||
/** \brief Set a double vector value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param vec The vector value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::vector<double>& vec);
|
||||
/** \brief Set a float vector value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param vec The vector value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::vector<float>& vec);
|
||||
/** \brief Set an integer vector value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param vec The vector value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::vector<int>& vec);
|
||||
/** \brief Set a bool vector value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param vec The vector value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::vector<bool>& vec);
|
||||
|
||||
/** \brief Set a string->string map value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param map The map value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::map<std::string, std::string>& map);
|
||||
/** \brief Set a string->double map value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param map The map value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::map<std::string, double>& map);
|
||||
/** \brief Set a string->float map value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param map The map value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::map<std::string, float>& map);
|
||||
/** \brief Set a string->int map value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param map The map value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::map<std::string, int>& map);
|
||||
/** \brief Set a string->bool map value on the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param map The map value to be inserted.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL void set(const std::string& key, const std::map<std::string, bool>& map);
|
||||
|
||||
|
||||
/** \brief Get a string value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] s Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::string& s);
|
||||
/** \brief Get a double value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] d Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, double& d);
|
||||
/** \brief Get a float value from the parameter server (internally using the double value).
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] f Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, float& f);
|
||||
/** \brief Get an integer value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] i Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, int& i);
|
||||
/** \brief Get a boolean value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] b Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, bool& b);
|
||||
/** \brief Get an arbitrary XML/RPC value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] v Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, XmlRpc::XmlRpcValue& v);
|
||||
|
||||
/** \brief Get a string value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] s Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::string& s);
|
||||
/** \brief Get a double value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] d Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, double& d);
|
||||
/** \brief Get a float value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] f Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, float& f);
|
||||
/** \brief Get an integer value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] i Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, int& i);
|
||||
/** \brief Get a boolean value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] b Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, bool& b);
|
||||
/** \brief Get an arbitrary XML/RPC value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] v Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, XmlRpc::XmlRpcValue& v);
|
||||
|
||||
/** \brief Get a string vector value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::vector<std::string>& vec);
|
||||
/** \brief Get a double vector value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::vector<double>& vec);
|
||||
/** \brief Get a float vector value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::vector<float>& vec);
|
||||
/** \brief Get an int vector value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::vector<int>& vec);
|
||||
/** \brief Get a bool vector value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::vector<bool>& vec);
|
||||
|
||||
/** \brief Get a string vector value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::vector<std::string>& vec);
|
||||
/** \brief Get a double vector value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::vector<double>& vec);
|
||||
/** \brief Get a float vector value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::vector<float>& vec);
|
||||
/** \brief Get an int vector value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::vector<int>& vec);
|
||||
/** \brief Get a bool vector value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] vec Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::vector<bool>& vec);
|
||||
|
||||
/** \brief Get a string->string map value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::map<std::string, std::string>& map);
|
||||
/** \brief Get a string->double map value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::map<std::string, double>& map);
|
||||
/** \brief Get a string->float map value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::map<std::string, float>& map);
|
||||
/** \brief Get a string->int map value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::map<std::string, int>& map);
|
||||
/** \brief Get a string->bool map value from the parameter server.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool get(const std::string& key, std::map<std::string, bool>& map);
|
||||
|
||||
/** \brief Get a string->string map value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::map<std::string, std::string>& map);
|
||||
/** \brief Get a string->double map value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::map<std::string, double>& map);
|
||||
/** \brief Get a string->float map value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::map<std::string, float>& map);
|
||||
/** \brief Get a string->int map value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::map<std::string, int>& map);
|
||||
/** \brief Get a string->bool map value from the parameter server, with local caching
|
||||
*
|
||||
* This function will cache parameters locally, and subscribe for updates from
|
||||
* the parameter server. Once the parameter is retrieved for the first time
|
||||
* no subsequent getCached() calls with the same key will query the master --
|
||||
* they will instead look up in the local cache.
|
||||
*
|
||||
* \param key The key to be used in the parameter server's dictionary
|
||||
* \param[out] map Storage for the retrieved value.
|
||||
*
|
||||
* \return true if the parameter value was retrieved, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool getCached(const std::string& key, std::map<std::string, bool>& map);
|
||||
|
||||
/** \brief Check whether a parameter exists on the parameter server.
|
||||
*
|
||||
* \param key The key to check.
|
||||
*
|
||||
* \return true if the parameter exists, false otherwise
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool has(const std::string& key);
|
||||
/** \brief Delete a parameter from the parameter server.
|
||||
*
|
||||
* \param key The key to delete.
|
||||
*
|
||||
* \return true if the deletion succeeded, false otherwise.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool del(const std::string& key);
|
||||
|
||||
/** \brief Search up the tree for a parameter with a given key
|
||||
*
|
||||
* This function parameter server's searchParam feature to search up the tree for
|
||||
* a parameter. For example, if the parameter server has a parameter [/a/b]
|
||||
* and you specify the namespace [/a/c/d], searching for the parameter "b" will
|
||||
* yield [/a/b]. If [/a/c/d/b] existed, that parameter would be returned instead.
|
||||
*
|
||||
* \param ns The namespace to begin the search in
|
||||
* \param key the parameter to search for
|
||||
* \param [out] result the found value (if any)
|
||||
*
|
||||
* \return true if the parameter was found, false otherwise.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool search(const std::string& ns, const std::string& key, std::string& result);
|
||||
|
||||
/** \brief Search up the tree for a parameter with a given key. This version defaults to starting in
|
||||
* the current node's name
|
||||
*
|
||||
* This function parameter server's searchParam feature to search up the tree for
|
||||
* a parameter. For example, if the parameter server has a parameter [/a/b]
|
||||
* and you specify the namespace [/a/c/d], searching for the parameter "b" will
|
||||
* yield [/a/b]. If [/a/c/d/b] existed, that parameter would be returned instead.
|
||||
*
|
||||
* \param key the parameter to search for
|
||||
* \param [out] result the found value (if any)
|
||||
*
|
||||
* \return true if the parameter was found, false otherwise.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
ROSCPP_DECL bool search(const std::string& key, std::string& result);
|
||||
|
||||
/**
|
||||
* \brief Get the list of all the parameters in the server
|
||||
* \param keys The vector of all the keys
|
||||
* \return false if the process fails
|
||||
*/
|
||||
ROSCPP_DECL bool getParamNames(std::vector<std::string>& keys);
|
||||
|
||||
/** \brief Assign value from parameter server, with default.
|
||||
*
|
||||
* This method tries to retrieve the indicated parameter value from the
|
||||
* parameter server, storing the result in param_val. If the value
|
||||
* cannot be retrieved from the server, default_val is used instead.
|
||||
*
|
||||
* \param param_name The key to be searched on the parameter server.
|
||||
* \param[out] param_val Storage for the retrieved value.
|
||||
* \param default_val Value to use if the server doesn't contain this
|
||||
* parameter.
|
||||
* \return true if the parameter was retrieved from the server, false otherwise.
|
||||
* \throws InvalidNameException if the key is not a valid graph resource name
|
||||
*/
|
||||
template<typename T>
|
||||
bool param(const std::string& param_name, T& param_val, const T& default_val)
|
||||
{
|
||||
if (has(param_name))
|
||||
{
|
||||
if (get(param_name, param_val))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
param_val = default_val;
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Return value from parameter server, or default if unavailable.
|
||||
*
|
||||
* This method tries to retrieve the indicated parameter value from the
|
||||
* parameter server. If the parameter cannot be retrieved, \c default_val
|
||||
* is returned instead.
|
||||
*
|
||||
* \param param_name The key to be searched on the parameter server.
|
||||
*
|
||||
* \param default_val Value to return if the server doesn't contain this
|
||||
* parameter.
|
||||
*
|
||||
* \return The parameter value retrieved from the parameter server, or
|
||||
* \c default_val if unavailable.
|
||||
*
|
||||
* \throws InvalidNameException If the key is not a valid graph resource name.
|
||||
*/
|
||||
template<typename T>
|
||||
T param(const std::string& param_name, const T& default_val)
|
||||
{
|
||||
T param_val;
|
||||
param(param_name, param_val, default_val);
|
||||
return param_val;
|
||||
}
|
||||
|
||||
} // namespace param
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_PARAM_H
|
||||
182
thirdparty/ros/ros_comm/clients/roscpp/include/ros/parameter_adapter.h
vendored
Normal file
182
thirdparty/ros/ros_comm/clients/roscpp/include/ros/parameter_adapter.h
vendored
Normal file
@@ -0,0 +1,182 @@
|
||||
/*
|
||||
* Copyright (C) 2010, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_PARAMETER_ADAPTER_H
|
||||
#define ROSCPP_PARAMETER_ADAPTER_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/message_event.h"
|
||||
#include <ros/static_assert.h>
|
||||
|
||||
#include <boost/type_traits/add_const.hpp>
|
||||
#include <boost/type_traits/remove_const.hpp>
|
||||
#include <boost/type_traits/remove_reference.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Generally not for outside use. Adapts a function parameter type into the message type, event type and parameter. Allows you to
|
||||
* retrieve a parameter type from an event type.
|
||||
*
|
||||
* ParameterAdapter is generally only useful for outside use when implementing things that require message callbacks
|
||||
* (such as the message_filters package)and you would like to support all the roscpp message parameter types
|
||||
*
|
||||
* The ParameterAdapter is templated on the callback parameter type (\b not the bare message type), and provides 3 things:
|
||||
* - Message typedef, which provides the bare message type, no const or reference qualifiers
|
||||
* - Event typedef, which provides the ros::MessageEvent type
|
||||
* - Parameter typedef, which provides the actual parameter type (may be slightly different from M)
|
||||
* - static getParameter(event) function, which returns a parameter type given the event
|
||||
* - static bool is_const informs you whether or not the parameter type is a const message
|
||||
*
|
||||
* ParameterAdapter is specialized to allow callbacks of any of the forms:
|
||||
\verbatim
|
||||
void callback(const boost::shared_ptr<M const>&);
|
||||
void callback(const boost::shared_ptr<M>&);
|
||||
void callback(boost::shared_ptr<M const>);
|
||||
void callback(boost::shared_ptr<M>);
|
||||
void callback(const M&);
|
||||
void callback(M);
|
||||
void callback(const MessageEvent<M const>&);
|
||||
void callback(const MessageEvent<M>&);
|
||||
\endverbatim
|
||||
*/
|
||||
template<typename M>
|
||||
struct ParameterAdapter
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef M Parameter;
|
||||
static const bool is_const = true;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return *event.getMessage();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
struct ParameterAdapter<const boost::shared_ptr<M const>& >
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef const boost::shared_ptr<Message const> Parameter;
|
||||
static const bool is_const = true;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return event.getMessage();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
struct ParameterAdapter<const boost::shared_ptr<M>& >
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef boost::shared_ptr<Message> Parameter;
|
||||
static const bool is_const = false;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return ros::MessageEvent<Message>(event).getMessage();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
struct ParameterAdapter<const M&>
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef const M& Parameter;
|
||||
static const bool is_const = true;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return *event.getMessage();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
struct ParameterAdapter<boost::shared_ptr<M const> >
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef boost::shared_ptr<Message const> Parameter;
|
||||
static const bool is_const = true;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return event.getMessage();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
struct ParameterAdapter<boost::shared_ptr<M> >
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef boost::shared_ptr<Message> Parameter;
|
||||
static const bool is_const = false;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return ros::MessageEvent<Message>(event).getMessage();
|
||||
}
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
struct ParameterAdapter<const ros::MessageEvent<M const>& >
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef const ros::MessageEvent<Message const>& Parameter;
|
||||
static const bool is_const = true;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return event;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
struct ParameterAdapter<const ros::MessageEvent<M>& >
|
||||
{
|
||||
typedef typename boost::remove_reference<typename boost::remove_const<M>::type>::type Message;
|
||||
typedef ros::MessageEvent<Message const> Event;
|
||||
typedef ros::MessageEvent<Message> Parameter;
|
||||
static const bool is_const = false;
|
||||
|
||||
static Parameter getParameter(const Event& event)
|
||||
{
|
||||
return ros::MessageEvent<Message>(event);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_PARAMETER_ADAPTER_H
|
||||
77
thirdparty/ros/ros_comm/clients/roscpp/include/ros/poll_manager.h
vendored
Normal file
77
thirdparty/ros/ros_comm/clients/roscpp/include/ros/poll_manager.h
vendored
Normal file
@@ -0,0 +1,77 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_POLL_MANAGER_H
|
||||
#define ROSCPP_POLL_MANAGER_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "poll_set.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/signals2.hpp>
|
||||
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
#include <boost/thread/thread.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class PollManager;
|
||||
typedef boost::shared_ptr<PollManager> PollManagerPtr;
|
||||
typedef boost::signals2::signal<void(void)> VoidSignal;
|
||||
typedef boost::function<void(void)> VoidFunc;
|
||||
|
||||
class ROSCPP_DECL PollManager
|
||||
{
|
||||
public:
|
||||
static const PollManagerPtr& instance();
|
||||
|
||||
PollManager();
|
||||
~PollManager();
|
||||
|
||||
PollSet& getPollSet() { return poll_set_; }
|
||||
|
||||
boost::signals2::connection addPollThreadListener(const VoidFunc& func);
|
||||
void removePollThreadListener(boost::signals2::connection c);
|
||||
|
||||
void start();
|
||||
void shutdown();
|
||||
private:
|
||||
void threadFunc();
|
||||
|
||||
PollSet poll_set_;
|
||||
volatile bool shutting_down_;
|
||||
|
||||
VoidSignal poll_signal_;
|
||||
boost::recursive_mutex signal_mutex_;
|
||||
|
||||
boost::thread thread_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
156
thirdparty/ros/ros_comm/clients/roscpp/include/ros/poll_set.h
vendored
Normal file
156
thirdparty/ros/ros_comm/clients/roscpp/include/ros/poll_set.h
vendored
Normal file
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_POLL_SET_H
|
||||
#define ROSCPP_POLL_SET_H
|
||||
|
||||
#include <vector>
|
||||
#include "io.h"
|
||||
#include "common.h"
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class Transport;
|
||||
typedef boost::shared_ptr<Transport> TransportPtr;
|
||||
|
||||
/**
|
||||
* \brief Manages a set of sockets being polled through the poll() function call.
|
||||
*
|
||||
* PollSet provides thread-safe ways of adding and deleting sockets, as well as adding
|
||||
* and deleting events.
|
||||
*/
|
||||
class ROSCPP_DECL PollSet
|
||||
{
|
||||
public:
|
||||
PollSet();
|
||||
~PollSet();
|
||||
|
||||
typedef boost::function<void(int)> SocketUpdateFunc;
|
||||
/**
|
||||
* \brief Add a socket.
|
||||
*
|
||||
* addSocket() may be called from any thread.
|
||||
* \param sock The socket to add
|
||||
* \param update_func The function to call when a socket has events
|
||||
* \param transport The (optional) transport associated with this socket. Mainly
|
||||
* used to prevent the transport from being deleted while we're calling the update function
|
||||
*/
|
||||
bool addSocket(int sock, const SocketUpdateFunc& update_func, const TransportPtr& transport = TransportPtr());
|
||||
/**
|
||||
* \brief Delete a socket
|
||||
*
|
||||
* delSocket() may be called from any thread.
|
||||
* \param sock The socket to delete
|
||||
*/
|
||||
bool delSocket(int sock);
|
||||
|
||||
/**
|
||||
* \brief Add events to be polled on a socket
|
||||
*
|
||||
* addEvents() may be called from any thread.
|
||||
* \param sock The socket to add events to
|
||||
* \param events The events to add
|
||||
*/
|
||||
bool addEvents(int sock, int events);
|
||||
/**
|
||||
* \brief Delete events to be polled on a socket
|
||||
*
|
||||
* delEvents() may be called from any thread.
|
||||
* \param sock The socket to delete events from
|
||||
* \param events The events to delete
|
||||
*/
|
||||
bool delEvents(int sock, int events);
|
||||
|
||||
/**
|
||||
* \brief Process all socket events
|
||||
*
|
||||
* This function will actually call poll() on the available sockets, and allow
|
||||
* them to do their processing.
|
||||
*
|
||||
* update() may only be called from one thread at a time
|
||||
*
|
||||
* \param poll_timeout The time, in milliseconds, for the poll() call to timeout after
|
||||
* if there are no events. Note that this does not provide an upper bound for the entire
|
||||
* function, just the call to poll()
|
||||
*/
|
||||
void update(int poll_timeout);
|
||||
|
||||
/**
|
||||
* \brief Signal our poll() call to finish if it's blocked waiting (see the poll_timeout
|
||||
* option for update()).
|
||||
*/
|
||||
void signal();
|
||||
|
||||
private:
|
||||
/**
|
||||
* \brief Creates the native pollset for our sockets, if any have changed
|
||||
*/
|
||||
void createNativePollset();
|
||||
|
||||
/**
|
||||
* \brief Called when events have been triggered on our signal pipe
|
||||
*/
|
||||
void onLocalPipeEvents(int events);
|
||||
|
||||
struct SocketInfo
|
||||
{
|
||||
TransportPtr transport_;
|
||||
SocketUpdateFunc func_;
|
||||
int fd_;
|
||||
int events_;
|
||||
};
|
||||
typedef std::map<int, SocketInfo> M_SocketInfo;
|
||||
M_SocketInfo socket_info_;
|
||||
boost::mutex socket_info_mutex_;
|
||||
bool sockets_changed_;
|
||||
|
||||
boost::mutex just_deleted_mutex_;
|
||||
typedef std::vector<int> V_int;
|
||||
V_int just_deleted_;
|
||||
|
||||
std::vector<socket_pollfd> ufds_;
|
||||
|
||||
boost::mutex signal_mutex_;
|
||||
signal_fd_t signal_pipe_[2];
|
||||
|
||||
int epfd_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_POLL_SET_H
|
||||
192
thirdparty/ros/ros_comm/clients/roscpp/include/ros/publication.h
vendored
Normal file
192
thirdparty/ros/ros_comm/clients/roscpp/include/ros/publication.h
vendored
Normal file
@@ -0,0 +1,192 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_PUBLICATION_H
|
||||
#define ROSCPP_PUBLICATION_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/advertise_options.h"
|
||||
#include "common.h"
|
||||
#include "xmlrpcpp/XmlRpc.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class SubscriberLink;
|
||||
typedef boost::shared_ptr<SubscriberLink> SubscriberLinkPtr;
|
||||
typedef std::vector<SubscriberLinkPtr> V_SubscriberLink;
|
||||
|
||||
/**
|
||||
* \brief A Publication manages an advertised topic
|
||||
*/
|
||||
class ROSCPP_DECL Publication
|
||||
{
|
||||
public:
|
||||
Publication(const std::string &name,
|
||||
const std::string& datatype,
|
||||
const std::string& _md5sum,
|
||||
const std::string& message_definition,
|
||||
size_t max_queue,
|
||||
bool latch,
|
||||
bool has_header);
|
||||
|
||||
~Publication();
|
||||
|
||||
void addCallbacks(const SubscriberCallbacksPtr& callbacks);
|
||||
void removeCallbacks(const SubscriberCallbacksPtr& callbacks);
|
||||
|
||||
/**
|
||||
* \brief queues an outgoing message into each of the publishers, so that it gets sent to every subscriber
|
||||
*/
|
||||
bool enqueueMessage(const SerializedMessage& m);
|
||||
/**
|
||||
* \brief returns the max queue size of this publication
|
||||
*/
|
||||
inline size_t getMaxQueue() { return max_queue_; }
|
||||
/**
|
||||
* \brief Get the accumulated stats for this publication
|
||||
*/
|
||||
XmlRpc::XmlRpcValue getStats();
|
||||
/**
|
||||
* \brief Get the accumulated info for this publication
|
||||
*/
|
||||
void getInfo(XmlRpc::XmlRpcValue& info);
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not this publication has any subscribers
|
||||
*/
|
||||
bool hasSubscribers();
|
||||
/**
|
||||
* \brief Returns the number of subscribers this publication has
|
||||
*/
|
||||
uint32_t getNumSubscribers();
|
||||
|
||||
void getPublishTypes(bool& serialize, bool& nocopy, const std::type_info& ti);
|
||||
|
||||
/**
|
||||
* \brief Returns the name of the topic this publication broadcasts to
|
||||
*/
|
||||
const std::string& getName() const { return name_; }
|
||||
/**
|
||||
* \brief Returns the data type of the message published by this publication
|
||||
*/
|
||||
const std::string& getDataType() const { return datatype_; }
|
||||
/**
|
||||
* \brief Returns the md5sum of the message published by this publication
|
||||
*/
|
||||
const std::string& getMD5Sum() const { return md5sum_; }
|
||||
/**
|
||||
* \brief Returns the full definition of the message published by this publication
|
||||
*/
|
||||
const std::string& getMessageDefinition() const { return message_definition_; }
|
||||
/**
|
||||
* \brief Returns the sequence number
|
||||
*/
|
||||
uint32_t getSequence() { return seq_; }
|
||||
|
||||
bool isLatched() { return latch_; }
|
||||
|
||||
/**
|
||||
* \brief Adds a publisher to our list
|
||||
*/
|
||||
void addSubscriberLink(const SubscriberLinkPtr& sub_link);
|
||||
/**
|
||||
* \brief Removes a publisher from our list (deleting it if it's the last reference)
|
||||
*/
|
||||
void removeSubscriberLink(const SubscriberLinkPtr& sub_link);
|
||||
|
||||
/**
|
||||
* \brief Drop this publication. Disconnects all publishers.
|
||||
*/
|
||||
void drop();
|
||||
/**
|
||||
* \brief Returns if this publication is valid or not
|
||||
*/
|
||||
bool isDropped() { return dropped_; }
|
||||
|
||||
uint32_t incrementSequence();
|
||||
|
||||
size_t getNumCallbacks();
|
||||
|
||||
bool isLatching() { return latch_; }
|
||||
|
||||
void publish(SerializedMessage& m);
|
||||
void processPublishQueue();
|
||||
|
||||
bool validateHeader(const Header& h, std::string& error_msg);
|
||||
|
||||
private:
|
||||
void dropAllConnections();
|
||||
|
||||
/**
|
||||
* \brief Called when a new peer has connected. Calls the connection callback
|
||||
*/
|
||||
void peerConnect(const SubscriberLinkPtr& sub_link);
|
||||
/**
|
||||
* \brief Called when a peer has disconnected. Calls the disconnection callback
|
||||
*/
|
||||
void peerDisconnect(const SubscriberLinkPtr& sub_link);
|
||||
|
||||
std::string name_;
|
||||
std::string datatype_;
|
||||
std::string md5sum_;
|
||||
std::string message_definition_;
|
||||
size_t max_queue_;
|
||||
uint32_t seq_;
|
||||
boost::mutex seq_mutex_;
|
||||
|
||||
typedef std::vector<SubscriberCallbacksPtr> V_Callback;
|
||||
V_Callback callbacks_;
|
||||
boost::mutex callbacks_mutex_;
|
||||
|
||||
V_SubscriberLink subscriber_links_;
|
||||
// We use a recursive mutex here for the rare case that a publish call causes another one (like in the case of a rosconsole call)
|
||||
boost::mutex subscriber_links_mutex_;
|
||||
|
||||
bool dropped_;
|
||||
|
||||
bool latch_;
|
||||
bool has_header_;
|
||||
SerializedMessage last_message_;
|
||||
|
||||
uint32_t intraprocess_subscriber_count_;
|
||||
|
||||
typedef std::vector<SerializedMessage> V_SerializedMessage;
|
||||
V_SerializedMessage publish_queue_;
|
||||
boost::mutex publish_queue_mutex_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_PUBLICATION_H
|
||||
203
thirdparty/ros/ros_comm/clients/roscpp/include/ros/publisher.h
vendored
Normal file
203
thirdparty/ros/ros_comm/clients/roscpp/include/ros/publisher.h
vendored
Normal file
@@ -0,0 +1,203 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_PUBLISHER_HANDLE_H
|
||||
#define ROSCPP_PUBLISHER_HANDLE_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/common.h"
|
||||
#include "ros/message.h"
|
||||
#include "ros/serialization.h"
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
/**
|
||||
* \brief Manages an advertisement on a specific topic.
|
||||
*
|
||||
* A Publisher should always be created through a call to NodeHandle::advertise(), or copied from one
|
||||
* that was. Once all copies of a specific
|
||||
* Publisher go out of scope, any subscriber status callbacks associated with that handle will stop
|
||||
* being called. Once all Publishers for a given topic go out of scope the topic will be unadvertised.
|
||||
*/
|
||||
class ROSCPP_DECL Publisher
|
||||
{
|
||||
public:
|
||||
Publisher() {}
|
||||
Publisher(const Publisher& rhs);
|
||||
~Publisher();
|
||||
|
||||
/**
|
||||
* \brief Publish a message on the topic associated with this Publisher.
|
||||
*
|
||||
* This version of publish will allow fast intra-process message-passing in the future,
|
||||
* so you may not mutate the message after it has been passed in here (since it will be
|
||||
* passed directly into a callback function)
|
||||
*
|
||||
*/
|
||||
template <typename M>
|
||||
void publish(const boost::shared_ptr<M>& message) const
|
||||
{
|
||||
using namespace serialization;
|
||||
|
||||
if (!impl_)
|
||||
{
|
||||
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!impl_->isValid())
|
||||
{
|
||||
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
ROS_ASSERT_MSG(impl_->md5sum_ == "*" || std::string(mt::md5sum<M>(*message)) == "*" || impl_->md5sum_ == mt::md5sum<M>(*message),
|
||||
"Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]",
|
||||
mt::datatype<M>(*message), mt::md5sum<M>(*message),
|
||||
impl_->datatype_.c_str(), impl_->md5sum_.c_str());
|
||||
|
||||
SerializedMessage m;
|
||||
m.type_info = &typeid(M);
|
||||
m.message = message;
|
||||
|
||||
publish(boost::bind(serializeMessage<M>, boost::ref(*message)), m);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Publish a message on the topic associated with this Publisher.
|
||||
*/
|
||||
template <typename M>
|
||||
void publish(const M& message) const
|
||||
{
|
||||
using namespace serialization;
|
||||
namespace mt = ros::message_traits;
|
||||
|
||||
if (!impl_)
|
||||
{
|
||||
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!impl_->isValid())
|
||||
{
|
||||
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
ROS_ASSERT_MSG(impl_->md5sum_ == "*" || std::string(mt::md5sum<M>(message)) == "*" || impl_->md5sum_ == mt::md5sum<M>(message),
|
||||
"Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]",
|
||||
mt::datatype<M>(message), mt::md5sum<M>(message),
|
||||
impl_->datatype_.c_str(), impl_->md5sum_.c_str());
|
||||
|
||||
SerializedMessage m;
|
||||
publish(boost::bind(serializeMessage<M>, boost::ref(message)), m);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Shutdown the advertisement associated with this Publisher
|
||||
*
|
||||
* This method usually does not need to be explicitly called, as automatic shutdown happens when
|
||||
* all copies of this Publisher go out of scope
|
||||
*
|
||||
* This method overrides the automatic reference counted unadvertise, and does so immediately.
|
||||
* \note Note that if multiple advertisements were made through NodeHandle::advertise(), this will
|
||||
* only remove the one associated with this Publisher
|
||||
*/
|
||||
void shutdown();
|
||||
|
||||
/**
|
||||
* \brief Returns the topic that this Publisher will publish on.
|
||||
*/
|
||||
std::string getTopic() const;
|
||||
|
||||
/**
|
||||
* \brief Returns the number of subscribers that are currently connected to this Publisher
|
||||
*/
|
||||
uint32_t getNumSubscribers() const;
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not this topic is latched
|
||||
*/
|
||||
bool isLatched() const;
|
||||
|
||||
operator void*() const { return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; }
|
||||
|
||||
bool operator<(const Publisher& rhs) const
|
||||
{
|
||||
return impl_ < rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator==(const Publisher& rhs) const
|
||||
{
|
||||
return impl_ == rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator!=(const Publisher& rhs) const
|
||||
{
|
||||
return impl_ != rhs.impl_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
Publisher(const std::string& topic, const std::string& md5sum,
|
||||
const std::string& datatype, const NodeHandle& node_handle,
|
||||
const SubscriberCallbacksPtr& callbacks);
|
||||
|
||||
void publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) const;
|
||||
void incrementSequence() const;
|
||||
|
||||
class ROSCPP_DECL Impl
|
||||
{
|
||||
public:
|
||||
Impl();
|
||||
~Impl();
|
||||
|
||||
void unadvertise();
|
||||
bool isValid() const;
|
||||
|
||||
std::string topic_;
|
||||
std::string md5sum_;
|
||||
std::string datatype_;
|
||||
NodeHandlePtr node_handle_;
|
||||
SubscriberCallbacksPtr callbacks_;
|
||||
bool unadvertised_;
|
||||
};
|
||||
typedef boost::shared_ptr<Impl> ImplPtr;
|
||||
typedef boost::weak_ptr<Impl> ImplWPtr;
|
||||
|
||||
ImplPtr impl_;
|
||||
|
||||
friend class NodeHandle;
|
||||
friend class NodeHandleBackingCollection;
|
||||
};
|
||||
|
||||
typedef std::vector<Publisher> V_Publisher;
|
||||
}
|
||||
|
||||
#endif // ROSCPP_PUBLISHER_HANDLE_H
|
||||
|
||||
109
thirdparty/ros/ros_comm/clients/roscpp/include/ros/publisher_link.h
vendored
Normal file
109
thirdparty/ros/ros_comm/clients/roscpp/include/ros/publisher_link.h
vendored
Normal file
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_PUBLISHER_LINK_H
|
||||
#define ROSCPP_PUBLISHER_LINK_H
|
||||
|
||||
#include "ros/common.h"
|
||||
#include "ros/transport_hints.h"
|
||||
#include "ros/header.h"
|
||||
#include "common.h"
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/weak_ptr.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
|
||||
#include <queue>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
class Header;
|
||||
class Message;
|
||||
class Subscription;
|
||||
typedef boost::shared_ptr<Subscription> SubscriptionPtr;
|
||||
typedef boost::weak_ptr<Subscription> SubscriptionWPtr;
|
||||
class Connection;
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
|
||||
/**
|
||||
* \brief Handles a connection to a single publisher on a given topic. Receives messages from a publisher
|
||||
* and hands them off to its parent Subscription
|
||||
*/
|
||||
class ROSCPP_DECL PublisherLink : public boost::enable_shared_from_this<PublisherLink>
|
||||
{
|
||||
public:
|
||||
class Stats
|
||||
{
|
||||
public:
|
||||
uint64_t bytes_received_, messages_received_, drops_;
|
||||
Stats()
|
||||
: bytes_received_(0), messages_received_(0), drops_(0) { }
|
||||
};
|
||||
|
||||
|
||||
PublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri, const TransportHints& transport_hints);
|
||||
virtual ~PublisherLink();
|
||||
|
||||
const Stats &getStats() { return stats_; }
|
||||
const std::string& getPublisherXMLRPCURI();
|
||||
int getConnectionID() const { return connection_id_; }
|
||||
const std::string& getCallerID() { return caller_id_; }
|
||||
bool isLatched() { return latched_; }
|
||||
|
||||
bool setHeader(const Header& header);
|
||||
|
||||
/**
|
||||
* \brief Handles handing off a received message to the subscription, where it will be deserialized and called back
|
||||
*/
|
||||
virtual void handleMessage(const SerializedMessage& m, bool ser, bool nocopy) = 0;
|
||||
virtual std::string getTransportType() = 0;
|
||||
virtual std::string getTransportInfo() = 0;
|
||||
virtual void drop() = 0;
|
||||
|
||||
const std::string& getMD5Sum();
|
||||
|
||||
protected:
|
||||
SubscriptionWPtr parent_;
|
||||
unsigned int connection_id_;
|
||||
std::string publisher_xmlrpc_uri_;
|
||||
|
||||
Stats stats_;
|
||||
|
||||
TransportHints transport_hints_;
|
||||
|
||||
bool latched_;
|
||||
std::string caller_id_;
|
||||
Header header_;
|
||||
std::string md5sum_;
|
||||
};
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_PUBLISHER_LINK_H
|
||||
|
||||
|
||||
|
||||
58
thirdparty/ros/ros_comm/clients/roscpp/include/ros/ros.h
vendored
Normal file
58
thirdparty/ros/ros_comm/clients/roscpp/include/ros/ros.h
vendored
Normal file
@@ -0,0 +1,58 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_ROS_H
|
||||
#define ROSCPP_ROS_H
|
||||
|
||||
#include "ros/time.h"
|
||||
#include "ros/rate.h"
|
||||
#include "ros/console.h"
|
||||
#include "ros/assert.h"
|
||||
|
||||
#include "ros/common.h"
|
||||
#include "ros/types.h"
|
||||
#include "ros/node_handle.h"
|
||||
#include "ros/publisher.h"
|
||||
#include "ros/single_subscriber_publisher.h"
|
||||
#include "ros/service_server.h"
|
||||
#include "ros/subscriber.h"
|
||||
#include "ros/service.h"
|
||||
#include "ros/init.h"
|
||||
#include "ros/master.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/param.h"
|
||||
#include "ros/topic.h"
|
||||
#include "ros/names.h"
|
||||
|
||||
#endif
|
||||
20
thirdparty/ros/ros_comm/clients/roscpp/include/ros/roscpp.dox
vendored
Normal file
20
thirdparty/ros/ros_comm/clients/roscpp/include/ros/roscpp.dox
vendored
Normal file
@@ -0,0 +1,20 @@
|
||||
/**
|
||||
@mainpage roscpp
|
||||
|
||||
@htmlinclude manifest.html
|
||||
|
||||
\b roscpp is a ROS client implementation in C++. The main parts of \b roscpp are:
|
||||
|
||||
- \ref ros::init() : A version of ros::init() must be called before using any of the rest of the ROS system.
|
||||
- \ref ros::NodeHandle : Public interface to topics, services, parameters, etc.
|
||||
- \ref ros::master : Contains functions for querying information from the master
|
||||
- \ref ros::this_node : Contains functions for querying information about this process' node
|
||||
- \ref ros::service : Contains functions for querying information about services
|
||||
- \ref ros::param : Contains functions for querying the parameter service without the need for a ros::NodeHandle
|
||||
- \ref ros::names : Contains functions for manipulating ROS graph resource names
|
||||
|
||||
@par Examples
|
||||
|
||||
Many examples of using ROS can be found <a href="http://www.ros.org/wiki/ROS/Tutorials">on the wiki</a> and in the <a href="http://www.ros.org/wiki/roscpp_tutorials">roscpp_tutorials</a> package.
|
||||
|
||||
*/
|
||||
84
thirdparty/ros/ros_comm/clients/roscpp/include/ros/rosout_appender.h
vendored
Normal file
84
thirdparty/ros/ros_comm/clients/roscpp/include/ros/rosout_appender.h
vendored
Normal file
@@ -0,0 +1,84 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_ROSOUT_APPENDER_H
|
||||
#define ROSCPP_ROSOUT_APPENDER_H
|
||||
|
||||
#include <ros/message_forward.h>
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/weak_ptr.hpp>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
namespace rosgraph_msgs
|
||||
{
|
||||
ROS_DECLARE_MESSAGE(Log);
|
||||
}
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class Publication;
|
||||
typedef boost::shared_ptr<Publication> PublicationPtr;
|
||||
typedef boost::weak_ptr<Publication> PublicationWPtr;
|
||||
|
||||
class ROSCPP_DECL ROSOutAppender : public ros::console::LogAppender
|
||||
{
|
||||
public:
|
||||
ROSOutAppender();
|
||||
~ROSOutAppender();
|
||||
|
||||
const std::string& getLastError() const;
|
||||
|
||||
virtual void log(::ros::console::Level level, const char* str, const char* file, const char* function, int line);
|
||||
|
||||
protected:
|
||||
void logThread();
|
||||
|
||||
std::string last_error_;
|
||||
|
||||
typedef std::vector<rosgraph_msgs::LogPtr> V_Log;
|
||||
V_Log log_queue_;
|
||||
boost::mutex queue_mutex_;
|
||||
boost::condition_variable queue_condition_;
|
||||
bool shutting_down_;
|
||||
|
||||
boost::thread publish_thread_;
|
||||
};
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif
|
||||
163
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service.h
vendored
Normal file
163
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service.h
vendored
Normal file
@@ -0,0 +1,163 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_H
|
||||
#define ROSCPP_SERVICE_H
|
||||
|
||||
#include <string>
|
||||
#include "ros/common.h"
|
||||
#include "ros/message.h"
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/node_handle.h"
|
||||
#include "ros/service_traits.h"
|
||||
#include "ros/names.h"
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class ServiceServerLink;
|
||||
typedef boost::shared_ptr<ServiceServerLink> ServiceServerLinkPtr;
|
||||
|
||||
/**
|
||||
* \brief Contains functions for querying information about and calling a service
|
||||
*/
|
||||
namespace service
|
||||
{
|
||||
|
||||
/** @brief Invoke an RPC service.
|
||||
*
|
||||
* This method invokes an RPC service on a remote server, looking up the
|
||||
* service location first via the master.
|
||||
*
|
||||
* @param service_name The name of the service.
|
||||
* @param req The request message.
|
||||
* @param[out] res Storage for the response message.
|
||||
*
|
||||
* @return true on success, false otherwise.
|
||||
*/
|
||||
template<class MReq, class MRes>
|
||||
bool call(const std::string& service_name, MReq& req, MRes& res)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
NodeHandle nh;
|
||||
ServiceClientOptions ops(ros::names::resolve(service_name), st::md5sum(req), false, M_string());
|
||||
ServiceClient client = nh.serviceClient(ops);
|
||||
return client.call(req, res);
|
||||
}
|
||||
|
||||
/** @brief Invoke an RPC service.
|
||||
*
|
||||
* This method invokes an RPC service on a remote server, looking up the
|
||||
* service location first via the master.
|
||||
*
|
||||
* @param service_name The name of the service.
|
||||
* @param service The service class that contains the request and response messages
|
||||
*
|
||||
* @return true on success, false otherwise.
|
||||
*/
|
||||
template<class Service>
|
||||
bool call(const std::string& service_name, Service& service)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
|
||||
NodeHandle nh;
|
||||
ServiceClientOptions ops(ros::names::resolve(service_name), st::md5sum(service), false, M_string());
|
||||
ServiceClient client = nh.serviceClient(ops);
|
||||
return client.call(service.request, service.response);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Wait for a service to be advertised and available. Blocks until it is.
|
||||
* \param service_name Name of the service to wait for.
|
||||
* \param timeout The amount of time to wait for, in milliseconds. If timeout is -1,
|
||||
* waits until the node is shutdown
|
||||
* \return true on success, false otherwise
|
||||
*/
|
||||
ROSCPP_DECL bool waitForService(const std::string& service_name, int32_t timeout);
|
||||
|
||||
/**
|
||||
* \brief Wait for a service to be advertised and available. Blocks until it is.
|
||||
* \param service_name Name of the service to wait for.
|
||||
* \param timeout The amount of time to wait for before timing out. If timeout is -1 (default),
|
||||
* waits until the node is shutdown
|
||||
* \return true on success, false otherwise
|
||||
*/
|
||||
ROSCPP_DECL bool waitForService(const std::string& service_name, ros::Duration timeout = ros::Duration(-1));
|
||||
|
||||
/**
|
||||
* \brief Checks if a service is both advertised and available.
|
||||
* \param service_name Name of the service to check for
|
||||
* \param print_failure_reason Whether to print the reason for failure to the console (service not advertised vs.
|
||||
* could not connect to the advertised host)
|
||||
* \return true if the service is up and available, false otherwise
|
||||
*/
|
||||
ROSCPP_DECL bool exists(const std::string& service_name, bool print_failure_reason);
|
||||
|
||||
/** @brief Create a client for a service.
|
||||
*
|
||||
* When the last handle reference of a persistent connection is cleared, the connection will automatically close.
|
||||
*
|
||||
* @param service_name The name of the service to connect to
|
||||
* @param persistent Whether this connection should persist. Persistent services keep the connection to the remote host active
|
||||
* so that subsequent calls will happen faster. In general persistent services are discouraged, as they are not as
|
||||
* robust to node failure as non-persistent services.
|
||||
* @param header_values Key/value pairs you'd like to send along in the connection handshake
|
||||
*/
|
||||
template<class MReq, class MRes>
|
||||
ServiceClient createClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string())
|
||||
{
|
||||
NodeHandle nh;
|
||||
ServiceClient client = nh.template serviceClient<MReq, MRes>(ros::names::resolve(service_name), persistent, header_values);
|
||||
return client;
|
||||
}
|
||||
|
||||
/** @brief Create a client for a service.
|
||||
*
|
||||
* When the last handle reference of a persistent connection is cleared, the connection will automatically close.
|
||||
*
|
||||
* @param service_name The name of the service to connect to
|
||||
* @param persistent Whether this connection should persist. Persistent services keep the connection to the remote host active
|
||||
* so that subsequent calls will happen faster. In general persistent services are discouraged, as they are not as
|
||||
* robust to node failure as non-persistent services.
|
||||
* @param header_values Key/value pairs you'd like to send along in the connection handshake
|
||||
*/
|
||||
template<class Service>
|
||||
ServiceClient createClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string())
|
||||
{
|
||||
NodeHandle nh;
|
||||
ServiceClient client = nh.template serviceClient<Service>(ros::names::resolve(service_name), persistent, header_values);
|
||||
return client;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_SERVICE_H
|
||||
|
||||
195
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_callback_helper.h
vendored
Normal file
195
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_callback_helper.h
vendored
Normal file
@@ -0,0 +1,195 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_MESSAGE_HELPER_H
|
||||
#define ROSCPP_SERVICE_MESSAGE_HELPER_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/common.h"
|
||||
#include "ros/message.h"
|
||||
#include "ros/message_traits.h"
|
||||
#include "ros/service_traits.h"
|
||||
#include "ros/serialization.h"
|
||||
|
||||
#include <boost/type_traits/is_base_of.hpp>
|
||||
#include <boost/utility/enable_if.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
struct ROSCPP_DECL ServiceCallbackHelperCallParams
|
||||
{
|
||||
SerializedMessage request;
|
||||
SerializedMessage response;
|
||||
boost::shared_ptr<M_string> connection_header;
|
||||
};
|
||||
|
||||
template<typename M>
|
||||
inline boost::shared_ptr<M> defaultServiceCreateFunction()
|
||||
{
|
||||
return boost::make_shared<M>();
|
||||
}
|
||||
|
||||
template<typename MReq, typename MRes>
|
||||
struct ServiceSpecCallParams
|
||||
{
|
||||
boost::shared_ptr<MReq> request;
|
||||
boost::shared_ptr<MRes> response;
|
||||
boost::shared_ptr<M_string> connection_header;
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Event type for services, ros::ServiceEvent<MReq, MRes>& can be used in your callback instead of MReq&, MRes&
|
||||
*
|
||||
* Useful if you need to retrieve meta-data about the call, such as the full connection header, or the caller's node name
|
||||
*/
|
||||
template<typename MReq, typename MRes>
|
||||
class ServiceEvent
|
||||
{
|
||||
public:
|
||||
typedef MReq RequestType;
|
||||
typedef MRes ResponseType;
|
||||
typedef boost::shared_ptr<RequestType> RequestPtr;
|
||||
typedef boost::shared_ptr<ResponseType> ResponsePtr;
|
||||
typedef boost::function<bool(ServiceEvent<RequestType, ResponseType>&)> CallbackType;
|
||||
|
||||
static bool call(const CallbackType& cb, ServiceSpecCallParams<RequestType, ResponseType>& params)
|
||||
{
|
||||
ServiceEvent<RequestType, ResponseType> event(params.request, params.response, params.connection_header);
|
||||
return cb(event);
|
||||
}
|
||||
|
||||
ServiceEvent(const boost::shared_ptr<MReq const>& req, const boost::shared_ptr<MRes>& res, const boost::shared_ptr<M_string>& connection_header)
|
||||
: request_(req)
|
||||
, response_(res)
|
||||
, connection_header_(connection_header)
|
||||
{}
|
||||
|
||||
/**
|
||||
* \brief Returns a const-reference to the request
|
||||
*/
|
||||
const RequestType& getRequest() const { return *request_; }
|
||||
/**
|
||||
* \brief Returns a non-const reference to the response
|
||||
*/
|
||||
ResponseType& getResponse() const { return *response_; }
|
||||
/**
|
||||
* \brief Returns a reference to the connection header.
|
||||
*/
|
||||
M_string& getConnectionHeader() const { return *connection_header_; }
|
||||
|
||||
/**
|
||||
* \brief Returns the name of the node which called this service
|
||||
*/
|
||||
const std::string& getCallerName() const { return (*connection_header_)["callerid"]; }
|
||||
private:
|
||||
boost::shared_ptr<RequestType const> request_;
|
||||
boost::shared_ptr<ResponseType> response_;
|
||||
boost::shared_ptr<M_string> connection_header_;
|
||||
};
|
||||
|
||||
template<typename MReq, typename MRes>
|
||||
struct ServiceSpec
|
||||
{
|
||||
typedef MReq RequestType;
|
||||
typedef MRes ResponseType;
|
||||
typedef boost::shared_ptr<RequestType> RequestPtr;
|
||||
typedef boost::shared_ptr<ResponseType> ResponsePtr;
|
||||
typedef boost::function<bool(RequestType&, ResponseType&)> CallbackType;
|
||||
|
||||
static bool call(const CallbackType& cb, ServiceSpecCallParams<RequestType, ResponseType>& params)
|
||||
{
|
||||
return cb(*params.request, *params.response);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Abstract base class used by service servers to deal with concrete message types through a common
|
||||
* interface. This is one part of the roscpp API that is \b not fully stable, so overloading this class
|
||||
* is not recommended
|
||||
*/
|
||||
class ROSCPP_DECL ServiceCallbackHelper
|
||||
{
|
||||
public:
|
||||
virtual ~ServiceCallbackHelper() {}
|
||||
virtual bool call(ServiceCallbackHelperCallParams& params) = 0;
|
||||
};
|
||||
typedef boost::shared_ptr<ServiceCallbackHelper> ServiceCallbackHelperPtr;
|
||||
|
||||
/**
|
||||
* \brief Concrete generic implementation of ServiceCallbackHelper for any normal service type
|
||||
*/
|
||||
template<typename Spec>
|
||||
class ServiceCallbackHelperT : public ServiceCallbackHelper
|
||||
{
|
||||
public:
|
||||
typedef typename Spec::RequestType RequestType;
|
||||
typedef typename Spec::ResponseType ResponseType;
|
||||
typedef typename Spec::RequestPtr RequestPtr;
|
||||
typedef typename Spec::ResponsePtr ResponsePtr;
|
||||
typedef typename Spec::CallbackType Callback;
|
||||
typedef boost::function<RequestPtr()> ReqCreateFunction;
|
||||
typedef boost::function<ResponsePtr()> ResCreateFunction;
|
||||
|
||||
ServiceCallbackHelperT(const Callback& callback,
|
||||
const ReqCreateFunction& create_req =
|
||||
// these static casts are legally unnecessary, but
|
||||
// here to keep clang 2.8 from getting confused
|
||||
static_cast<RequestPtr(*)()>(defaultServiceCreateFunction<RequestType>),
|
||||
const ResCreateFunction& create_res =
|
||||
static_cast<ResponsePtr(*)()>(defaultServiceCreateFunction<ResponseType>))
|
||||
: callback_(callback)
|
||||
, create_req_(create_req)
|
||||
, create_res_(create_res)
|
||||
{
|
||||
}
|
||||
|
||||
virtual bool call(ServiceCallbackHelperCallParams& params)
|
||||
{
|
||||
namespace ser = serialization;
|
||||
RequestPtr req(create_req_());
|
||||
ResponsePtr res(create_res_());
|
||||
|
||||
ser::deserializeMessage(params.request, *req);
|
||||
|
||||
ServiceSpecCallParams<RequestType, ResponseType> call_params;
|
||||
call_params.request = req;
|
||||
call_params.response = res;
|
||||
call_params.connection_header = params.connection_header;
|
||||
bool ok = Spec::call(callback_, call_params);
|
||||
params.response = ser::serializeServiceResponse(ok, *res);
|
||||
return ok;
|
||||
}
|
||||
|
||||
private:
|
||||
Callback callback_;
|
||||
ReqCreateFunction create_req_;
|
||||
ResCreateFunction create_res_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_SERVICE_MESSAGE_HELPER_H
|
||||
215
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_client.h
vendored
Normal file
215
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_client.h
vendored
Normal file
@@ -0,0 +1,215 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_CLIENT_H
|
||||
#define ROSCPP_SERVICE_CLIENT_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/common.h"
|
||||
#include "ros/service_traits.h"
|
||||
#include "ros/serialization.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Provides a handle-based interface to service client connections
|
||||
*/
|
||||
class ROSCPP_DECL ServiceClient
|
||||
{
|
||||
public:
|
||||
ServiceClient() {}
|
||||
ServiceClient(const std::string& service_name, bool persistent, const M_string& header_values, const std::string& service_md5sum);
|
||||
ServiceClient(const ServiceClient& rhs);
|
||||
~ServiceClient();
|
||||
|
||||
/**
|
||||
* @brief Call the service aliased by this handle with the specified request/response messages.
|
||||
* @note The request/response message types must match the types specified in the templated call to NodeHandle::serviceClient()/service::createClient()
|
||||
*/
|
||||
template<class MReq, class MRes>
|
||||
bool call(MReq& req, MRes& res)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
|
||||
if (!isValid())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (strcmp(st::md5sum(req), st::md5sum(res)))
|
||||
{
|
||||
ROS_ERROR("The request and response parameters to the service "
|
||||
"call must be autogenerated from the same "
|
||||
"server definition file (.srv). your service call "
|
||||
"for %s appeared to use request/response types "
|
||||
"from different .srv files. (%s vs. %s)", impl_->name_.c_str(), st::md5sum(req), st::md5sum(res));
|
||||
return false;
|
||||
}
|
||||
|
||||
return call(req, res, st::md5sum(req));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Call the service aliased by this handle with the specified service request/response
|
||||
*/
|
||||
template<class Service>
|
||||
bool call(Service& service)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
|
||||
if (!isValid())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return call(service.request, service.response, st::md5sum(service));
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Mostly for internal use, the other templated versions of call() just call into this one
|
||||
*/
|
||||
template<typename MReq, typename MRes>
|
||||
bool call(const MReq& req, MRes& resp, const std::string& service_md5sum)
|
||||
{
|
||||
namespace ser = serialization;
|
||||
SerializedMessage ser_req = ser::serializeMessage(req);
|
||||
SerializedMessage ser_resp;
|
||||
bool ok = call(ser_req, ser_resp, service_md5sum);
|
||||
if (!ok)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
ser::deserializeMessage(ser_resp, resp);
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
deserializeFailed(e);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool call(const SerializedMessage& req, SerializedMessage& resp, const std::string& service_md5sum);
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not this handle is valid. For a persistent service, this becomes false when the connection has dropped.
|
||||
* Non-persistent service handles are always valid.
|
||||
*/
|
||||
bool isValid() const;
|
||||
|
||||
/**
|
||||
* \brief Returns true if this handle points to a persistent service, false otherwise.
|
||||
*/
|
||||
bool isPersistent() const;
|
||||
|
||||
/**
|
||||
* \brief Shutdown the connection associated with this ServiceClient
|
||||
*
|
||||
* This method usually does not need to be explicitly called, as automatic shutdown happens when
|
||||
* all copies of this ServiceClient go out of scope
|
||||
*
|
||||
* This method overrides the automatic reference counted shutdown, and does so immediately.
|
||||
*/
|
||||
void shutdown();
|
||||
|
||||
/**
|
||||
* \brief Wait for this service to be advertised and available. Blocks until it is.
|
||||
* \param timeout The amount of time to wait for before timing out. If timeout is -1 (default),
|
||||
* waits until the node is shutdown
|
||||
* \return true on success, false otherwise
|
||||
*/
|
||||
bool waitForExistence(ros::Duration timeout = ros::Duration(-1));
|
||||
|
||||
/**
|
||||
* \brief Checks if this is both advertised and available.
|
||||
* \return true if the service is up and available, false otherwise
|
||||
*/
|
||||
bool exists();
|
||||
|
||||
/**
|
||||
* \brief Returns the name of the service this ServiceClient connects to
|
||||
*/
|
||||
std::string getService();
|
||||
|
||||
operator void*() const { return isValid() ? (void*)1 : (void*)0; }
|
||||
bool operator<(const ServiceClient& rhs) const
|
||||
{
|
||||
return impl_ < rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator==(const ServiceClient& rhs) const
|
||||
{
|
||||
return impl_ == rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator!=(const ServiceClient& rhs) const
|
||||
{
|
||||
return impl_ != rhs.impl_;
|
||||
}
|
||||
|
||||
private:
|
||||
// This works around a problem with the OSX linker that causes the static variable declared by
|
||||
// ROS_ERROR to error with missing symbols when it's used directly in the templated call() method above
|
||||
// This for some reason only showed up in the rxtools package
|
||||
void deserializeFailed(const std::exception& e)
|
||||
{
|
||||
ROS_ERROR("Exception thrown while while deserializing service call: %s", e.what());
|
||||
}
|
||||
|
||||
struct Impl
|
||||
{
|
||||
Impl();
|
||||
~Impl();
|
||||
|
||||
void shutdown();
|
||||
bool isValid() const;
|
||||
|
||||
ServiceServerLinkPtr server_link_;
|
||||
std::string name_;
|
||||
bool persistent_;
|
||||
M_string header_values_;
|
||||
std::string service_md5sum_;
|
||||
bool is_shutdown_;
|
||||
};
|
||||
typedef boost::shared_ptr<Impl> ImplPtr;
|
||||
typedef boost::weak_ptr<Impl> ImplWPtr;
|
||||
|
||||
ImplPtr impl_;
|
||||
|
||||
friend class NodeHandle;
|
||||
friend class NodeHandleBackingCollection;
|
||||
};
|
||||
typedef boost::shared_ptr<ServiceClient> ServiceClientPtr;
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
91
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_client_link.h
vendored
Normal file
91
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_client_link.h
vendored
Normal file
@@ -0,0 +1,91 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_CLIENT_LINK_H
|
||||
#define ROSCPP_SERVICE_CLIENT_LINK_H
|
||||
|
||||
#include "ros/common.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
#include <boost/signals2/connection.hpp>
|
||||
|
||||
#include <queue>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
class Header;
|
||||
class ServicePublication;
|
||||
typedef boost::weak_ptr<ServicePublication> ServicePublicationWPtr;
|
||||
typedef boost::shared_ptr<ServicePublication> ServicePublicationPtr;
|
||||
class Connection;
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
|
||||
/**
|
||||
* \brief Handles a connection to a single incoming service client.
|
||||
*/
|
||||
class ROSCPP_DECL ServiceClientLink : public boost::enable_shared_from_this<ServiceClientLink>
|
||||
{
|
||||
public:
|
||||
ServiceClientLink();
|
||||
virtual ~ServiceClientLink();
|
||||
|
||||
//
|
||||
bool initialize(const ConnectionPtr& connection);
|
||||
bool handleHeader(const Header& header);
|
||||
|
||||
/**
|
||||
* \brief Writes a response to the current request.
|
||||
* \param ok Whether the callback was successful or not
|
||||
* \param resp The message response. ServiceClientLink will delete this
|
||||
*/
|
||||
void processResponse(bool ok, const SerializedMessage& res);
|
||||
|
||||
const ConnectionPtr& getConnection() { return connection_; }
|
||||
|
||||
private:
|
||||
void onConnectionDropped(const ConnectionPtr& conn);
|
||||
|
||||
void onHeaderWritten(const ConnectionPtr& conn);
|
||||
void onRequestLength(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
void onRequest(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
void onResponseWritten(const ConnectionPtr& conn);
|
||||
|
||||
ConnectionPtr connection_;
|
||||
ServicePublicationWPtr parent_;
|
||||
bool persistent_;
|
||||
boost::signals2::connection dropped_conn_;
|
||||
};
|
||||
typedef boost::shared_ptr<ServiceClientLink> ServiceClientLinkPtr;
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_PUBLISHER_DATA_HANDLER_H
|
||||
|
||||
|
||||
|
||||
109
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_client_options.h
vendored
Normal file
109
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_client_options.h
vendored
Normal file
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_CLIENT_OPTIONS_H
|
||||
#define ROSCPP_SERVICE_CLIENT_OPTIONS_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "common.h"
|
||||
#include "ros/service_traits.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Encapsulates all options available for creating a ServiceClient
|
||||
*/
|
||||
struct ROSCPP_DECL ServiceClientOptions
|
||||
{
|
||||
ServiceClientOptions()
|
||||
: persistent(false)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Constructor
|
||||
* \param _service Name of the service to connect to
|
||||
* \param _md5sum md5sum of the service
|
||||
* \param _persistent Whether or not to keep the connection open to the service for future calls
|
||||
* \param _header Any extra values to be passed along in the connection header
|
||||
*/
|
||||
ServiceClientOptions(const std::string& _service, const std::string& _md5sum, bool _persistent, const M_string& _header)
|
||||
: service(_service)
|
||||
, md5sum(_md5sum)
|
||||
, persistent(_persistent)
|
||||
, header(_header)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Templated helper method, preventing you from needing to manually get the service md5sum
|
||||
* \param MReq [template] Request message type
|
||||
* \param MRes [template] Response message type
|
||||
* \param _service Name of the service to connect to
|
||||
* \param _persistent Whether or not to keep the connection open to the service for future calls
|
||||
* \param _header Any extra values to be passed along in the connection header
|
||||
*/
|
||||
template <class MReq, class MRes>
|
||||
void init(const std::string& _service, bool _persistent, const M_string& _header)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
|
||||
service = _service;
|
||||
md5sum = st::md5sum<MReq>();
|
||||
persistent = _persistent;
|
||||
header = _header;
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Templated helper method, preventing you from needing to manually get the service md5sum
|
||||
* \param Service [template] Service type
|
||||
* \param _service Name of the service to connect to
|
||||
* \param _persistent Whether or not to keep the connection open to the service for future calls
|
||||
* \param _header Any extra values to be passed along in the connection header
|
||||
*/
|
||||
template <class Service>
|
||||
void init(const std::string& _service, bool _persistent, const M_string& _header)
|
||||
{
|
||||
namespace st = service_traits;
|
||||
|
||||
service = _service;
|
||||
md5sum = st::md5sum<Service>();
|
||||
persistent = _persistent;
|
||||
header = _header;
|
||||
}
|
||||
|
||||
std::string service; ///< Service to connect to
|
||||
std::string md5sum; ///< Service md5sum
|
||||
bool persistent; ///< Whether or not the connection should persist
|
||||
M_string header; ///< Extra key/value pairs to add to the connection header
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
145
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_manager.h
vendored
Normal file
145
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_manager.h
vendored
Normal file
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_MANAGER_H
|
||||
#define ROSCPP_SERVICE_MANAGER_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "common.h"
|
||||
#include "advertise_service_options.h"
|
||||
#include "service_client_options.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class ServiceManager;
|
||||
typedef boost::shared_ptr<ServiceManager> ServiceManagerPtr;
|
||||
|
||||
class PollManager;
|
||||
typedef boost::shared_ptr<PollManager> PollManagerPtr;
|
||||
|
||||
class XMLRPCManager;
|
||||
typedef boost::shared_ptr<XMLRPCManager> XMLRPCManagerPtr;
|
||||
|
||||
class ConnectionManager;
|
||||
typedef boost::shared_ptr<ConnectionManager> ConnectionManagerPtr;
|
||||
|
||||
class ROSCPP_DECL ServiceManager
|
||||
{
|
||||
public:
|
||||
static const ServiceManagerPtr& instance();
|
||||
|
||||
ServiceManager();
|
||||
~ServiceManager();
|
||||
|
||||
/** @brief Lookup an advertised service.
|
||||
*
|
||||
* This method iterates over advertised_services, looking for one with name
|
||||
* matching the given topic name. The advertised_services_mutex is locked
|
||||
* during this search. This method is only used internally.
|
||||
*
|
||||
* @param service The service name to look for.
|
||||
*
|
||||
* @returns Pointer to the matching ServicePublication, NULL if none is found.
|
||||
*/
|
||||
ServicePublicationPtr lookupServicePublication(const std::string& service);
|
||||
|
||||
/** @brief Create a new client to the specified service. If a client to that service already exists, returns the existing one.
|
||||
*
|
||||
* @param service The service to connect to
|
||||
* @param persistent Whether to keep this connection alive for more than one service call
|
||||
* @param request_md5sum The md5sum of the request message
|
||||
* @param response_md5sum The md5sum of the response message
|
||||
*
|
||||
* @returns Shared pointer to the ServiceServerLink, empty shared pointer if none is found.
|
||||
*/
|
||||
ServiceServerLinkPtr createServiceServerLink(const std::string& service,
|
||||
bool persistent,
|
||||
const std::string& request_md5sum, const std::string& response_md5sum,
|
||||
const M_string& header_values);
|
||||
|
||||
/** @brief Remove the specified service client from our list
|
||||
*
|
||||
* @param client The client to remove
|
||||
*/
|
||||
void removeServiceServerLink(const ServiceServerLinkPtr& client);
|
||||
|
||||
/** @brief Lookup the host/port of a service.
|
||||
*
|
||||
* @param name The name of the service
|
||||
* @param serv_host OUT -- The host of the service
|
||||
* @param serv_port OUT -- The port of the service
|
||||
*/
|
||||
bool lookupService(const std::string& name, std::string& serv_host, uint32_t& serv_port);
|
||||
|
||||
/** @brief Unadvertise a service.
|
||||
*
|
||||
* This call unadvertises a service, which must have been previously
|
||||
* advertised, using advertiseService().
|
||||
*
|
||||
* After this call completes, it is guaranteed that no further
|
||||
* callbacks will be invoked for this service.
|
||||
*
|
||||
* This method can be safely called from within a service callback.
|
||||
*
|
||||
* @param serv_name The service to be unadvertised.
|
||||
*
|
||||
* @return true on successful unadvertisement, false otherwise.
|
||||
*/
|
||||
bool unadvertiseService(const std::string& serv_name);
|
||||
|
||||
bool advertiseService(const AdvertiseServiceOptions& ops);
|
||||
|
||||
void start();
|
||||
void shutdown();
|
||||
private:
|
||||
|
||||
bool isServiceAdvertised(const std::string& serv_name);
|
||||
bool unregisterService(const std::string& service);
|
||||
|
||||
bool isShuttingDown() { return shutting_down_; }
|
||||
|
||||
L_ServicePublication service_publications_;
|
||||
boost::mutex service_publications_mutex_;
|
||||
|
||||
L_ServiceServerLink service_server_links_;
|
||||
boost::mutex service_server_links_mutex_;
|
||||
|
||||
volatile bool shutting_down_;
|
||||
boost::recursive_mutex shutting_down_mutex_;
|
||||
|
||||
PollManagerPtr poll_manager_;
|
||||
ConnectionManagerPtr connection_manager_;
|
||||
XMLRPCManagerPtr xmlrpc_manager_;
|
||||
};
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_SERVICE_MANAGER_H
|
||||
121
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_publication.h
vendored
Normal file
121
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_publication.h
vendored
Normal file
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_PUBLICATION_H
|
||||
#define ROSCPP_SERVICE_PUBLICATION_H
|
||||
|
||||
#include "ros/service_callback_helper.h"
|
||||
#include "common.h"
|
||||
#include "xmlrpcpp/XmlRpc.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
|
||||
#include <vector>
|
||||
#include <queue>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class ServiceClientLink;
|
||||
typedef boost::shared_ptr<ServiceClientLink> ServiceClientLinkPtr;
|
||||
typedef std::vector<ServiceClientLinkPtr> V_ServiceClientLink;
|
||||
class CallbackQueueInterface;
|
||||
|
||||
class Message;
|
||||
|
||||
/**
|
||||
* \brief Manages an advertised service.
|
||||
*
|
||||
* ServicePublication manages all incoming service requests. If its thread pool size is not 0, it will queue the requests
|
||||
* into a number of threads, calling the callback from within those threads. Otherwise it immediately calls the callback
|
||||
*/
|
||||
class ROSCPP_DECL ServicePublication : public boost::enable_shared_from_this<ServicePublication>
|
||||
{
|
||||
public:
|
||||
ServicePublication(const std::string& name, const std::string &md5sum, const std::string& data_type, const std::string& request_data_type,
|
||||
const std::string& response_data_type, const ServiceCallbackHelperPtr& helper, CallbackQueueInterface* queue,
|
||||
const VoidConstPtr& tracked_object);
|
||||
~ServicePublication();
|
||||
|
||||
/**
|
||||
* \brief Adds a request to the queue if our thread pool size is not 0, otherwise immediately calls the callback
|
||||
*/
|
||||
void processRequest(boost::shared_array<uint8_t> buf, size_t num_bytes, const ServiceClientLinkPtr& link);
|
||||
|
||||
/**
|
||||
* \brief Adds a service link for us to manage
|
||||
*/
|
||||
void addServiceClientLink(const ServiceClientLinkPtr& link);
|
||||
/**
|
||||
* \brief Removes a service link from our list
|
||||
*/
|
||||
void removeServiceClientLink(const ServiceClientLinkPtr& link);
|
||||
|
||||
/**
|
||||
* \brief Terminate this service server
|
||||
*/
|
||||
void drop();
|
||||
/**
|
||||
* \brief Returns whether or not this service server is valid
|
||||
*/
|
||||
bool isDropped() { return dropped_; }
|
||||
|
||||
const std::string& getMD5Sum() { return md5sum_; }
|
||||
const std::string& getRequestDataType() { return request_data_type_; }
|
||||
const std::string& getResponseDataType() { return response_data_type_; }
|
||||
const std::string& getDataType() { return data_type_; }
|
||||
const std::string& getName() { return name_; }
|
||||
|
||||
private:
|
||||
void dropAllConnections();
|
||||
|
||||
std::string name_;
|
||||
std::string md5sum_;
|
||||
std::string data_type_;
|
||||
std::string request_data_type_;
|
||||
std::string response_data_type_;
|
||||
ServiceCallbackHelperPtr helper_;
|
||||
|
||||
V_ServiceClientLink client_links_;
|
||||
boost::mutex client_links_mutex_;
|
||||
|
||||
bool dropped_;
|
||||
|
||||
CallbackQueueInterface* callback_queue_;
|
||||
bool has_tracked_object_;
|
||||
VoidConstWPtr tracked_object_;
|
||||
};
|
||||
typedef boost::shared_ptr<ServicePublication> ServicePublicationPtr;
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_SERVICE_PUBLICATION_H
|
||||
112
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_server.h
vendored
Normal file
112
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_server.h
vendored
Normal file
@@ -0,0 +1,112 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_HANDLE_H
|
||||
#define ROSCPP_SERVICE_HANDLE_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "common.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Manages an service advertisement.
|
||||
*
|
||||
* A ServiceServer should always be created through a call to NodeHandle::advertiseService(), or copied from
|
||||
* one that was. Once all copies of a specific
|
||||
* ServiceServer go out of scope, the service associated with it will be unadvertised and the service callback
|
||||
* will stop being called.
|
||||
*/
|
||||
class ROSCPP_DECL ServiceServer
|
||||
{
|
||||
public:
|
||||
ServiceServer() {}
|
||||
ServiceServer(const ServiceServer& rhs);
|
||||
~ServiceServer();
|
||||
|
||||
/**
|
||||
* \brief Unadvertise the service associated with this ServiceServer
|
||||
*
|
||||
* This method usually does not need to be explicitly called, as automatic shutdown happens when
|
||||
* all copies of this ServiceServer go out of scope
|
||||
*
|
||||
* This method overrides the automatic reference counted unadvertise, and immediately
|
||||
* unadvertises the service associated with this ServiceServer
|
||||
*/
|
||||
void shutdown();
|
||||
|
||||
std::string getService() const;
|
||||
|
||||
operator void*() const { return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; }
|
||||
|
||||
bool operator<(const ServiceServer& rhs) const
|
||||
{
|
||||
return impl_ < rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator==(const ServiceServer& rhs) const
|
||||
{
|
||||
return impl_ == rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator!=(const ServiceServer& rhs) const
|
||||
{
|
||||
return impl_ != rhs.impl_;
|
||||
}
|
||||
|
||||
private:
|
||||
ServiceServer(const std::string& service, const NodeHandle& node_handle);
|
||||
|
||||
class Impl
|
||||
{
|
||||
public:
|
||||
Impl();
|
||||
~Impl();
|
||||
|
||||
void unadvertise();
|
||||
bool isValid() const;
|
||||
|
||||
std::string service_;
|
||||
NodeHandlePtr node_handle_;
|
||||
bool unadvertised_;
|
||||
};
|
||||
typedef boost::shared_ptr<Impl> ImplPtr;
|
||||
typedef boost::weak_ptr<Impl> ImplWPtr;
|
||||
|
||||
ImplPtr impl_;
|
||||
|
||||
friend class NodeHandle;
|
||||
friend class NodeHandleBackingCollection;
|
||||
};
|
||||
typedef std::vector<ServiceServer> V_ServiceServer;
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_SERVICE_HANDLE_H
|
||||
|
||||
|
||||
162
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_server_link.h
vendored
Normal file
162
thirdparty/ros/ros_comm/clients/roscpp/include/ros/service_server_link.h
vendored
Normal file
@@ -0,0 +1,162 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SERVICE_SERVER_LINK_H
|
||||
#define ROSCPP_SERVICE_SERVER_LINK_H
|
||||
|
||||
#include "ros/common.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
#include <queue>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
class Header;
|
||||
class Message;
|
||||
class Connection;
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
|
||||
/**
|
||||
* \brief Handles a connection to a service. If it's a non-persistent client, automatically disconnects
|
||||
* when its first service call has finished.
|
||||
*/
|
||||
class ROSCPP_DECL ServiceServerLink : public boost::enable_shared_from_this<ServiceServerLink>
|
||||
{
|
||||
private:
|
||||
struct CallInfo
|
||||
{
|
||||
SerializedMessage req_;
|
||||
SerializedMessage* resp_;
|
||||
|
||||
bool finished_;
|
||||
boost::condition_variable finished_condition_;
|
||||
boost::mutex finished_mutex_;
|
||||
boost::thread::id caller_thread_id_;
|
||||
|
||||
bool success_;
|
||||
bool call_finished_;
|
||||
|
||||
std::string exception_string_;
|
||||
};
|
||||
typedef boost::shared_ptr<CallInfo> CallInfoPtr;
|
||||
typedef std::queue<CallInfoPtr> Q_CallInfo;
|
||||
|
||||
public:
|
||||
typedef std::map<std::string, std::string> M_string;
|
||||
ServiceServerLink(const std::string& service_name, bool persistent, const std::string& request_md5sum, const std::string& response_md5sum, const M_string& header_values);
|
||||
virtual ~ServiceServerLink();
|
||||
|
||||
//
|
||||
bool initialize(const ConnectionPtr& connection);
|
||||
|
||||
/**
|
||||
* \brief Returns whether this client is still valid, ie. its connection has not been dropped
|
||||
*/
|
||||
bool isValid() const;
|
||||
/**
|
||||
* \brief Returns whether this is a persistent connection
|
||||
*/
|
||||
bool isPersistent() const { return persistent_; }
|
||||
|
||||
const ConnectionPtr& getConnection() const { return connection_; }
|
||||
|
||||
const std::string& getServiceName() const { return service_name_; }
|
||||
const std::string& getRequestMD5Sum() const { return request_md5sum_; }
|
||||
const std::string& getResponseMD5Sum() const { return response_md5sum_; }
|
||||
|
||||
/**
|
||||
* \brief Blocking call the service this client is connected to
|
||||
*
|
||||
* If there is already a call happening in another thread, this will queue up the call and still block until
|
||||
* it has finished.
|
||||
*/
|
||||
bool call(const SerializedMessage& req, SerializedMessage& resp);
|
||||
|
||||
private:
|
||||
void onConnectionDropped(const ConnectionPtr& conn);
|
||||
bool onHeaderReceived(const ConnectionPtr& conn, const Header& header);
|
||||
|
||||
/**
|
||||
* \brief Called when the currently queued call has finished. Clears out the current call, notifying it that it
|
||||
* has finished, then calls processNextCall()
|
||||
*/
|
||||
void callFinished();
|
||||
/**
|
||||
* \brief Pops the next call off the queue if one is available. If this is a non-persistent connection and the queue is empty
|
||||
* it will also drop the connection.
|
||||
*/
|
||||
void processNextCall();
|
||||
/**
|
||||
* \brief Clear all calls, notifying them that they've failed
|
||||
*/
|
||||
void clearCalls();
|
||||
/**
|
||||
* \brief Cancel a queued call, notifying it that it has failed
|
||||
*/
|
||||
void cancelCall(const CallInfoPtr& info);
|
||||
|
||||
void onHeaderWritten(const ConnectionPtr& conn);
|
||||
void onRequestWritten(const ConnectionPtr& conn);
|
||||
void onResponseOkAndLength(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
void onResponse(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
|
||||
ConnectionPtr connection_;
|
||||
std::string service_name_;
|
||||
bool persistent_;
|
||||
std::string request_md5sum_;
|
||||
std::string response_md5sum_;
|
||||
|
||||
M_string extra_outgoing_header_values_;
|
||||
bool header_written_;
|
||||
bool header_read_;
|
||||
|
||||
Q_CallInfo call_queue_;
|
||||
boost::mutex call_queue_mutex_;
|
||||
|
||||
CallInfoPtr current_call_;
|
||||
|
||||
bool dropped_;
|
||||
};
|
||||
typedef boost::shared_ptr<ServiceServerLink> ServiceServerLinkPtr;
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_SERVICE_SERVER_LINK_H
|
||||
|
||||
|
||||
|
||||
107
thirdparty/ros/ros_comm/clients/roscpp/include/ros/single_subscriber_publisher.h
vendored
Normal file
107
thirdparty/ros/ros_comm/clients/roscpp/include/ros/single_subscriber_publisher.h
vendored
Normal file
@@ -0,0 +1,107 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SINGLE_SUBSCRIBER_PUBLISHER_H
|
||||
#define ROSCPP_SINGLE_SUBSCRIBER_PUBLISHER_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/serialization.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/utility.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Allows publication of a message to a single subscriber. Only available inside subscriber connection callbacks
|
||||
*/
|
||||
class ROSCPP_DECL SingleSubscriberPublisher : public boost::noncopyable
|
||||
{
|
||||
public:
|
||||
SingleSubscriberPublisher(const SubscriberLinkPtr& link);
|
||||
~SingleSubscriberPublisher();
|
||||
|
||||
/**
|
||||
* \brief Publish a message on the topic associated with this Publisher.
|
||||
*
|
||||
* This version of publish will allow fast intra-process message-passing in the future,
|
||||
* so you may not mutate the message after it has been passed in here (since it will be
|
||||
* passed directly into a callback function)
|
||||
*
|
||||
*/
|
||||
template<class M>
|
||||
void publish(const boost::shared_ptr<M const>& message) const
|
||||
{
|
||||
publish(*message);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Publish a message on the topic associated with this Publisher.
|
||||
*
|
||||
* This version of publish will allow fast intra-process message-passing in the future,
|
||||
* so you may not mutate the message after it has been passed in here (since it will be
|
||||
* passed directly into a callback function)
|
||||
*
|
||||
*/
|
||||
template<class M>
|
||||
void publish(const boost::shared_ptr<M>& message) const
|
||||
{
|
||||
publish(*message);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Publish a message on the topic associated with this Publisher.
|
||||
*/
|
||||
template<class M>
|
||||
void publish(const M& message) const
|
||||
{
|
||||
using namespace serialization;
|
||||
SerializedMessage m = serializeMessage(message);
|
||||
publish(m);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Returns the topic this publisher publishes on
|
||||
*/
|
||||
std::string getTopic() const;
|
||||
|
||||
/**
|
||||
* \brief Returns the name of the subscriber node
|
||||
*/
|
||||
std::string getSubscriberName() const;
|
||||
|
||||
private:
|
||||
void publish(const SerializedMessage& m) const;
|
||||
|
||||
SubscriberLinkPtr link_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_PUBLISHER_HANDLE_H
|
||||
|
||||
135
thirdparty/ros/ros_comm/clients/roscpp/include/ros/spinner.h
vendored
Normal file
135
thirdparty/ros/ros_comm/clients/roscpp/include/ros/spinner.h
vendored
Normal file
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SPINNER_H
|
||||
#define ROSCPP_SPINNER_H
|
||||
|
||||
#include "ros/types.h"
|
||||
#include "common.h"
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
class NodeHandle;
|
||||
class CallbackQueue;
|
||||
|
||||
/**
|
||||
* \brief Abstract interface for classes which spin on a callback queue.
|
||||
*/
|
||||
class ROSCPP_DECL Spinner
|
||||
{
|
||||
public:
|
||||
virtual ~Spinner() {}
|
||||
|
||||
/**
|
||||
* \brief Spin on a callback queue (defaults to the global one). Blocks until roscpp has been shutdown.
|
||||
*/
|
||||
virtual void spin(CallbackQueue* queue = 0) = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Spinner which runs in a single thread.
|
||||
*/
|
||||
class SingleThreadedSpinner : public Spinner
|
||||
{
|
||||
public:
|
||||
virtual void spin(CallbackQueue* queue = 0);
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Spinner which spins in multiple threads.
|
||||
*/
|
||||
class ROSCPP_DECL MultiThreadedSpinner : public Spinner
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* \param thread_count Number of threads to use for calling callbacks. 0 will
|
||||
* automatically use however many hardware threads exist on your system.
|
||||
*/
|
||||
MultiThreadedSpinner(uint32_t thread_count = 0);
|
||||
|
||||
virtual void spin(CallbackQueue* queue = 0);
|
||||
|
||||
private:
|
||||
uint32_t thread_count_;
|
||||
};
|
||||
|
||||
class AsyncSpinnerImpl;
|
||||
typedef boost::shared_ptr<AsyncSpinnerImpl> AsyncSpinnerImplPtr;
|
||||
|
||||
/**
|
||||
* \brief AsyncSpinner is a spinner that does not conform to the abstract Spinner interface. Instead,
|
||||
* it spins asynchronously when you call start(), and stops when either you call stop(), ros::shutdown()
|
||||
* is called, or its destructor is called
|
||||
*
|
||||
* AsyncSpinner is reference counted internally, so if you copy one it will continue spinning until all
|
||||
* copies have destructed (or stop() has been called on one of them)
|
||||
*/
|
||||
class ROSCPP_DECL AsyncSpinner
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* \brief Simple constructor. Uses the global callback queue
|
||||
* \param thread_count The number of threads to use. A value of 0 means to use the number of processor cores
|
||||
*/
|
||||
AsyncSpinner(uint32_t thread_count);
|
||||
|
||||
/**
|
||||
* \brief Constructor with custom callback queue
|
||||
* \param thread_count The number of threads to use. A value of 0 means to use the number of processor cores
|
||||
* \param queue The callback queue to operate on. A null value means to use the global queue
|
||||
*/
|
||||
AsyncSpinner(uint32_t thread_count, CallbackQueue* queue);
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* \brief Check if the spinner can be started. The spinner shouldn't be started if
|
||||
* another single-threaded spinner is already operating on the callback queue.
|
||||
*
|
||||
* This function is not necessary anymore. start() will always try to start spinning
|
||||
* and throw a std::runtime_error if it failed.
|
||||
*/
|
||||
// TODO: deprecate in L-turtle
|
||||
bool canStart();
|
||||
/**
|
||||
* \brief Start this spinner spinning asynchronously
|
||||
*/
|
||||
void start();
|
||||
/**
|
||||
* \brief Stop this spinner from running
|
||||
*/
|
||||
void stop();
|
||||
|
||||
private:
|
||||
AsyncSpinnerImplPtr impl_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_SPIN_POLICY_H
|
||||
110
thirdparty/ros/ros_comm/clients/roscpp/include/ros/statistics.h
vendored
Normal file
110
thirdparty/ros/ros_comm/clients/roscpp/include/ros/statistics.h
vendored
Normal file
@@ -0,0 +1,110 @@
|
||||
/*
|
||||
* Copyright (C) 2013-2014, Dariush Forouher
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_STATISTICS_H
|
||||
#define ROSCPP_STATISTICS_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "poll_set.h"
|
||||
#include "common.h"
|
||||
#include "publisher.h"
|
||||
#include <ros/time.h>
|
||||
#include "ros/subscription_callback_helper.h"
|
||||
#include <map>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief This class logs statistics data about a ROS connection and
|
||||
* publishs them periodically on a common topic.
|
||||
*
|
||||
* It provides a callback() function that has to be called everytime
|
||||
* a new message arrives on a topic.
|
||||
*/
|
||||
class ROSCPP_DECL StatisticsLogger
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructior
|
||||
*/
|
||||
StatisticsLogger();
|
||||
|
||||
/**
|
||||
* Actual initialization. Must be called before the first call to callback()
|
||||
*/
|
||||
void init(const SubscriptionCallbackHelperPtr& helper);
|
||||
|
||||
/**
|
||||
* Callback function. Must be called for every message received.
|
||||
*/
|
||||
void callback(const boost::shared_ptr<M_string>& connection_header, const std::string& topic, const std::string& callerid, const SerializedMessage& m, const uint64_t& bytes_sent, const ros::Time& received_time, bool dropped);
|
||||
|
||||
private:
|
||||
|
||||
// these are hard constrains
|
||||
int max_window;
|
||||
int min_window;
|
||||
|
||||
// these are soft constrains
|
||||
int max_elements;
|
||||
int min_elements;
|
||||
|
||||
bool enable_statistics;
|
||||
|
||||
// remember, if this message type has a header
|
||||
bool hasHeader_;
|
||||
|
||||
// frequency to publish statistics
|
||||
double pub_frequency_;
|
||||
|
||||
// publisher for statistics data
|
||||
ros::Publisher pub_;
|
||||
|
||||
struct StatData {
|
||||
// last time, we published /statistics data
|
||||
ros::Time last_publish;
|
||||
// arrival times of all messages within the current window
|
||||
std::list<ros::Time> arrival_time_list;
|
||||
// age of all messages within the current window (if available)
|
||||
std::list<ros::Duration> age_list;
|
||||
// number of dropped messages
|
||||
uint64_t dropped_msgs;
|
||||
// latest sequence number observered (if available)
|
||||
uint64_t last_seq;
|
||||
// latest total traffic volume observed
|
||||
uint64_t stat_bytes_last;
|
||||
};
|
||||
|
||||
// storage for statistics data
|
||||
std::map<std::string, struct StatData> map_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
127
thirdparty/ros/ros_comm/clients/roscpp/include/ros/steady_timer.h
vendored
Normal file
127
thirdparty/ros/ros_comm/clients/roscpp/include/ros/steady_timer.h
vendored
Normal file
@@ -0,0 +1,127 @@
|
||||
/*
|
||||
* Copyright (C) 2017, Felix Ruess, Roboception GmbH
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_STEADY_TIMER_H
|
||||
#define ROSCPP_STEADY_TIMER_H
|
||||
|
||||
#include "common.h"
|
||||
#include "forwards.h"
|
||||
#include "steady_timer_options.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Manages a steady-clock timer callback
|
||||
*
|
||||
* A SteadyTimer should always be created through a call to NodeHandle::createSteadyTimer(), or copied from one
|
||||
* that was. Once all copies of a specific
|
||||
* SteadyTimer go out of scope, the callback associated with that handle will stop
|
||||
* being called.
|
||||
*/
|
||||
class ROSCPP_DECL SteadyTimer
|
||||
{
|
||||
public:
|
||||
SteadyTimer() {}
|
||||
SteadyTimer(const SteadyTimer& rhs);
|
||||
~SteadyTimer();
|
||||
|
||||
/**
|
||||
* \brief Start the timer. Does nothing if the timer is already started.
|
||||
*/
|
||||
void start();
|
||||
/**
|
||||
* \brief Stop the timer. Once this call returns, no more callbacks will be called. Does
|
||||
* nothing if the timer is already stopped.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not the timer has any pending events to call.
|
||||
*/
|
||||
bool hasPending();
|
||||
|
||||
/**
|
||||
* \brief Set the period of this timer
|
||||
*/
|
||||
void setPeriod(const WallDuration& period, bool reset=true);
|
||||
|
||||
bool isValid() { return impl_ && impl_->isValid(); }
|
||||
operator void*() { return isValid() ? (void *) 1 : (void *) 0; }
|
||||
|
||||
bool operator<(const SteadyTimer& rhs)
|
||||
{
|
||||
return impl_ < rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator==(const SteadyTimer& rhs)
|
||||
{
|
||||
return impl_ == rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator!=(const SteadyTimer& rhs)
|
||||
{
|
||||
return impl_ != rhs.impl_;
|
||||
}
|
||||
|
||||
private:
|
||||
SteadyTimer(const SteadyTimerOptions& ops);
|
||||
|
||||
class Impl
|
||||
{
|
||||
public:
|
||||
Impl();
|
||||
~Impl();
|
||||
|
||||
bool isValid();
|
||||
bool hasPending();
|
||||
void setPeriod(const WallDuration &period, bool reset=true);
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
|
||||
bool started_;
|
||||
int32_t timer_handle_;
|
||||
|
||||
WallDuration period_;
|
||||
SteadyTimerCallback callback_;
|
||||
CallbackQueueInterface *callback_queue_;
|
||||
VoidConstWPtr tracked_object_;
|
||||
bool has_tracked_object_;
|
||||
bool oneshot_;
|
||||
};
|
||||
typedef boost::shared_ptr<Impl> ImplPtr;
|
||||
typedef boost::weak_ptr<Impl> ImplWPtr;
|
||||
|
||||
ImplPtr impl_;
|
||||
|
||||
friend class NodeHandle;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
86
thirdparty/ros/ros_comm/clients/roscpp/include/ros/steady_timer_options.h
vendored
Normal file
86
thirdparty/ros/ros_comm/clients/roscpp/include/ros/steady_timer_options.h
vendored
Normal file
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Copyright (C) 2017, Felix Ruess, Roboception GmbH
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_STEADY_TIMER_OPTIONS_H
|
||||
#define ROSCPP_STEADY_TIMER_OPTIONS_H
|
||||
|
||||
#include "common.h"
|
||||
#include "ros/forwards.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Encapsulates all options available for starting a timer
|
||||
*/
|
||||
struct ROSCPP_DECL SteadyTimerOptions
|
||||
{
|
||||
SteadyTimerOptions()
|
||||
: period(0.1)
|
||||
, callback_queue(0)
|
||||
, oneshot(false)
|
||||
, autostart(true)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Constructor
|
||||
* \param
|
||||
*/
|
||||
SteadyTimerOptions(WallDuration _period, const SteadyTimerCallback& _callback, CallbackQueueInterface* _queue,
|
||||
bool oneshot = false, bool autostart = true)
|
||||
: period(_period)
|
||||
, callback(_callback)
|
||||
, callback_queue(_queue)
|
||||
, oneshot(oneshot)
|
||||
, autostart(autostart)
|
||||
{}
|
||||
|
||||
WallDuration period; ///< The period to call the callback at
|
||||
SteadyTimerCallback callback; ///< The callback to call
|
||||
|
||||
CallbackQueueInterface* callback_queue; ///< Queue to add callbacks to. If NULL, the global callback queue will be used
|
||||
|
||||
/**
|
||||
* A shared pointer to an object to track for these callbacks. If set, the a weak_ptr will be created to this object,
|
||||
* and if the reference count goes to 0 the subscriber callbacks will not get called.
|
||||
*
|
||||
* \note Note that setting this will cause a new reference to be added to the object before the
|
||||
* callback, and for it to go out of scope (and potentially be deleted) in the code path (and therefore
|
||||
* thread) that the callback is invoked from.
|
||||
*/
|
||||
VoidConstPtr tracked_object;
|
||||
|
||||
bool oneshot;
|
||||
bool autostart;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
170
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscribe_options.h
vendored
Normal file
170
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscribe_options.h
vendored
Normal file
@@ -0,0 +1,170 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SUBSCRIBE_OPTIONS_H
|
||||
#define ROSCPP_SUBSCRIBE_OPTIONS_H
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "common.h"
|
||||
#include "ros/transport_hints.h"
|
||||
#include "ros/message_traits.h"
|
||||
#include "subscription_callback_helper.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Encapsulates all options available for creating a Subscriber
|
||||
*/
|
||||
struct ROSCPP_DECL SubscribeOptions
|
||||
{
|
||||
/**
|
||||
*
|
||||
*/
|
||||
SubscribeOptions()
|
||||
: queue_size(1)
|
||||
, callback_queue(0)
|
||||
, allow_concurrent_callbacks(false)
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Constructor
|
||||
* \param _topic Topic to subscribe on
|
||||
* \param _queue_size Number of incoming messages to queue up for
|
||||
* processing (messages in excess of this queue capacity will be
|
||||
* discarded).
|
||||
* \param _md5sum
|
||||
* \param _datatype
|
||||
*/
|
||||
SubscribeOptions(const std::string& _topic, uint32_t _queue_size, const std::string& _md5sum, const std::string& _datatype)
|
||||
: topic(_topic)
|
||||
, queue_size(_queue_size)
|
||||
, md5sum(_md5sum)
|
||||
, datatype(_datatype)
|
||||
, callback_queue(0)
|
||||
, allow_concurrent_callbacks(false)
|
||||
{}
|
||||
|
||||
/**
|
||||
* \brief Templated initialization, templated on callback parameter type. Supports any callback parameters supported by the SubscriptionCallbackAdapter
|
||||
* \param _topic Topic to subscribe on
|
||||
* \param _queue_size Number of incoming messages to queue up for
|
||||
* processing (messages in excess of this queue capacity will be
|
||||
* discarded).
|
||||
* \param _callback Callback to call when a message arrives on this topic
|
||||
*/
|
||||
template<class P>
|
||||
void initByFullCallbackType(const std::string& _topic, uint32_t _queue_size,
|
||||
const boost::function<void (P)>& _callback,
|
||||
const boost::function<boost::shared_ptr<typename ParameterAdapter<P>::Message>(void)>& factory_fn = DefaultMessageCreator<typename ParameterAdapter<P>::Message>())
|
||||
{
|
||||
typedef typename ParameterAdapter<P>::Message MessageType;
|
||||
topic = _topic;
|
||||
queue_size = _queue_size;
|
||||
md5sum = message_traits::md5sum<MessageType>();
|
||||
datatype = message_traits::datatype<MessageType>();
|
||||
helper = boost::make_shared<SubscriptionCallbackHelperT<P> >(_callback, factory_fn);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Templated initialization, templated on message type. Only supports "const boost::shared_ptr<M const>&" callback types
|
||||
* \param _topic Topic to subscribe on
|
||||
* \param _queue_size Number of incoming messages to queue up for
|
||||
* processing (messages in excess of this queue capacity will be
|
||||
* discarded).
|
||||
* \param _callback Callback to call when a message arrives on this topic
|
||||
*/
|
||||
template<class M>
|
||||
void init(const std::string& _topic, uint32_t _queue_size,
|
||||
const boost::function<void (const boost::shared_ptr<M const>&)>& _callback,
|
||||
const boost::function<boost::shared_ptr<M>(void)>& factory_fn = DefaultMessageCreator<M>())
|
||||
{
|
||||
typedef typename ParameterAdapter<M>::Message MessageType;
|
||||
topic = _topic;
|
||||
queue_size = _queue_size;
|
||||
md5sum = message_traits::md5sum<MessageType>();
|
||||
datatype = message_traits::datatype<MessageType>();
|
||||
helper = boost::make_shared<SubscriptionCallbackHelperT<const boost::shared_ptr<MessageType const>&> >(_callback, factory_fn);
|
||||
}
|
||||
|
||||
std::string topic; ///< Topic to subscribe to
|
||||
uint32_t queue_size; ///< Number of incoming messages to queue up for processing (messages in excess of this queue capacity will be discarded).
|
||||
|
||||
std::string md5sum; ///< MD5 of the message datatype
|
||||
std::string datatype; ///< Datatype of the message we'd like to subscribe as
|
||||
|
||||
SubscriptionCallbackHelperPtr helper; ///< Helper object used to get create messages and call callbacks
|
||||
|
||||
CallbackQueueInterface* callback_queue; ///< Queue to add callbacks to. If NULL, the global callback queue will be used
|
||||
|
||||
/// By default subscription callbacks are guaranteed to arrive in-order, with only one callback happening for this subscription at any given
|
||||
/// time. Setting this to true allows you to receive multiple messages on the same topic from multiple threads at the same time
|
||||
bool allow_concurrent_callbacks;
|
||||
|
||||
/**
|
||||
* \brief An object whose destruction will prevent the callback associated with this subscription
|
||||
*
|
||||
* A shared pointer to an object to track for these callbacks. If set, the a weak_ptr will be created to this object,
|
||||
* and if the reference count goes to 0 the subscriber callbacks will not get called.
|
||||
*
|
||||
* \note Note that setting this will cause a new reference to be added to the object before the
|
||||
* callback, and for it to go out of scope (and potentially be deleted) in the code path (and therefore
|
||||
* thread) that the callback is invoked from.
|
||||
*/
|
||||
VoidConstPtr tracked_object;
|
||||
|
||||
TransportHints transport_hints; ///< Hints for transport type and options
|
||||
|
||||
/**
|
||||
* \brief Templated helper function for creating an AdvertiseServiceOptions with most of its options
|
||||
* \param topic Topic name to subscribe to
|
||||
* \param queue_size Number of incoming messages to queue up for
|
||||
* processing (messages in excess of this queue capacity will be
|
||||
* discarded).
|
||||
* \param callback The callback to invoke when a message is received on this topic
|
||||
* \param tracked_object The tracked object to use (see SubscribeOptions::tracked_object)
|
||||
* \param queue The callback queue to use (see SubscribeOptions::callback_queue)
|
||||
*/
|
||||
template<class M>
|
||||
static SubscribeOptions create(const std::string& topic, uint32_t queue_size,
|
||||
const boost::function<void (const boost::shared_ptr<M const>&)>& callback,
|
||||
const VoidConstPtr& tracked_object, CallbackQueueInterface* queue)
|
||||
{
|
||||
SubscribeOptions ops;
|
||||
ops.init<M>(topic, queue_size, callback);
|
||||
ops.tracked_object = tracked_object;
|
||||
ops.callback_queue = queue;
|
||||
return ops;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
121
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscriber.h
vendored
Normal file
121
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscriber.h
vendored
Normal file
@@ -0,0 +1,121 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SUBSCRIBER_HANDLE_H
|
||||
#define ROSCPP_SUBSCRIBER_HANDLE_H
|
||||
|
||||
#include "common.h"
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/subscription_callback_helper.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Manages an subscription callback on a specific topic.
|
||||
*
|
||||
* A Subscriber should always be created through a call to NodeHandle::subscribe(), or copied from one
|
||||
* that was. Once all copies of a specific
|
||||
* Subscriber go out of scope, the subscription callback associated with that handle will stop
|
||||
* being called. Once all Subscriber for a given topic go out of scope the topic will be unsubscribed.
|
||||
*/
|
||||
class ROSCPP_DECL Subscriber
|
||||
{
|
||||
public:
|
||||
Subscriber() {}
|
||||
Subscriber(const Subscriber& rhs);
|
||||
~Subscriber();
|
||||
|
||||
/**
|
||||
* \brief Unsubscribe the callback associated with this Subscriber
|
||||
*
|
||||
* This method usually does not need to be explicitly called, as automatic shutdown happens when
|
||||
* all copies of this Subscriber go out of scope
|
||||
*
|
||||
* This method overrides the automatic reference counted unsubscribe, and immediately
|
||||
* unsubscribes the callback associated with this Subscriber
|
||||
*/
|
||||
void shutdown();
|
||||
|
||||
std::string getTopic() const;
|
||||
|
||||
/**
|
||||
* \brief Returns the number of publishers this subscriber is connected to
|
||||
*/
|
||||
uint32_t getNumPublishers() const;
|
||||
|
||||
operator void*() const { return (impl_ && impl_->isValid()) ? (void*)1 : (void*)0; }
|
||||
|
||||
bool operator<(const Subscriber& rhs) const
|
||||
{
|
||||
return impl_ < rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator==(const Subscriber& rhs) const
|
||||
{
|
||||
return impl_ == rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator!=(const Subscriber& rhs) const
|
||||
{
|
||||
return impl_ != rhs.impl_;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
Subscriber(const std::string& topic, const NodeHandle& node_handle,
|
||||
const SubscriptionCallbackHelperPtr& helper);
|
||||
|
||||
class Impl
|
||||
{
|
||||
public:
|
||||
Impl();
|
||||
~Impl();
|
||||
|
||||
void unsubscribe();
|
||||
bool isValid() const;
|
||||
|
||||
std::string topic_;
|
||||
NodeHandlePtr node_handle_;
|
||||
SubscriptionCallbackHelperPtr helper_;
|
||||
bool unsubscribed_;
|
||||
};
|
||||
typedef boost::shared_ptr<Impl> ImplPtr;
|
||||
typedef boost::weak_ptr<Impl> ImplWPtr;
|
||||
|
||||
ImplPtr impl_;
|
||||
|
||||
friend class NodeHandle;
|
||||
friend class NodeHandleBackingCollection;
|
||||
};
|
||||
typedef std::vector<Subscriber> V_Subscriber;
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_PUBLISHER_HANDLE_H
|
||||
|
||||
|
||||
100
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscriber_link.h
vendored
Normal file
100
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscriber_link.h
vendored
Normal file
@@ -0,0 +1,100 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SUBSCRIBER_LINK_H
|
||||
#define ROSCPP_SUBSCRIBER_LINK_H
|
||||
|
||||
#include "ros/common.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/weak_ptr.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
|
||||
#include <queue>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
class Header;
|
||||
class Message;
|
||||
class Publication;
|
||||
typedef boost::shared_ptr<Publication> PublicationPtr;
|
||||
typedef boost::weak_ptr<Publication> PublicationWPtr;
|
||||
class Connection;
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
|
||||
class ROSCPP_DECL SubscriberLink : public boost::enable_shared_from_this<SubscriberLink>
|
||||
{
|
||||
public:
|
||||
class Stats
|
||||
{
|
||||
public:
|
||||
uint64_t bytes_sent_, message_data_sent_, messages_sent_;
|
||||
Stats()
|
||||
: bytes_sent_(0), message_data_sent_(0), messages_sent_(0) { }
|
||||
};
|
||||
|
||||
SubscriberLink();
|
||||
virtual ~SubscriberLink();
|
||||
|
||||
const std::string& getTopic() const { return topic_; }
|
||||
const Stats &getStats() { return stats_; }
|
||||
const std::string &getDestinationCallerID() const { return destination_caller_id_; }
|
||||
int getConnectionID() const { return connection_id_; }
|
||||
|
||||
/**
|
||||
* \brief Queue up a message for publication. Throws out old messages if we've reached our Publication's max queue size
|
||||
*/
|
||||
virtual void enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy) = 0;
|
||||
|
||||
virtual void drop() = 0;
|
||||
|
||||
virtual std::string getTransportType() = 0;
|
||||
virtual std::string getTransportInfo() = 0;
|
||||
|
||||
virtual bool isIntraprocess() { return false; }
|
||||
virtual void getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti) { (void)ti; ser = true; nocopy = false; }
|
||||
|
||||
const std::string& getMD5Sum();
|
||||
const std::string& getDataType();
|
||||
const std::string& getMessageDefinition();
|
||||
|
||||
protected:
|
||||
bool verifyDatatype(const std::string &datatype);
|
||||
|
||||
PublicationWPtr parent_;
|
||||
unsigned int connection_id_;
|
||||
std::string destination_caller_id_;
|
||||
Stats stats_;
|
||||
std::string topic_;
|
||||
};
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_SUBSCRIBER_LINK_H
|
||||
|
||||
|
||||
249
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscription.h
vendored
Normal file
249
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscription.h
vendored
Normal file
@@ -0,0 +1,249 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SUBSCRIPTION_H
|
||||
#define ROSCPP_SUBSCRIPTION_H
|
||||
|
||||
#include <queue>
|
||||
#include "ros/common.h"
|
||||
#include "ros/header.h"
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/transport_hints.h"
|
||||
#include "ros/xmlrpc_manager.h"
|
||||
#include "ros/statistics.h"
|
||||
#include "xmlrpcpp/XmlRpc.h"
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class PublisherLink;
|
||||
typedef boost::shared_ptr<PublisherLink> PublisherLinkPtr;
|
||||
|
||||
class SubscriptionCallback;
|
||||
typedef boost::shared_ptr<SubscriptionCallback> SubscriptionCallbackPtr;
|
||||
|
||||
class SubscriptionQueue;
|
||||
typedef boost::shared_ptr<SubscriptionQueue> SubscriptionQueuePtr;
|
||||
|
||||
class MessageDeserializer;
|
||||
typedef boost::shared_ptr<MessageDeserializer> MessageDeserializerPtr;
|
||||
|
||||
class SubscriptionCallbackHelper;
|
||||
typedef boost::shared_ptr<SubscriptionCallbackHelper> SubscriptionCallbackHelperPtr;
|
||||
|
||||
/**
|
||||
* \brief Manages a subscription on a single topic.
|
||||
*/
|
||||
class ROSCPP_DECL Subscription : public boost::enable_shared_from_this<Subscription>
|
||||
{
|
||||
public:
|
||||
Subscription(const std::string &name, const std::string& md5sum, const std::string& datatype, const TransportHints& transport_hints);
|
||||
virtual ~Subscription();
|
||||
|
||||
/**
|
||||
* \brief Terminate all our PublisherLinks
|
||||
*/
|
||||
void drop();
|
||||
/**
|
||||
* \brief Terminate all our PublisherLinks and join our callback thread if it exists
|
||||
*/
|
||||
void shutdown();
|
||||
/**
|
||||
* \brief Handle a publisher update list received from the master. Creates/drops PublisherLinks based on
|
||||
* the list. Never handles new self-subscriptions
|
||||
*/
|
||||
bool pubUpdate(const std::vector<std::string> &pubs);
|
||||
/**
|
||||
* \brief Negotiates a connection with a publisher
|
||||
* \param xmlrpc_uri The XMLRPC URI to connect to to negotiate the connection
|
||||
*/
|
||||
bool negotiateConnection(const std::string& xmlrpc_uri);
|
||||
|
||||
void addLocalConnection(const PublicationPtr& pub);
|
||||
|
||||
/**
|
||||
* \brief Returns whether this Subscription has been dropped or not
|
||||
*/
|
||||
bool isDropped() { return dropped_; }
|
||||
XmlRpc::XmlRpcValue getStats();
|
||||
void getInfo(XmlRpc::XmlRpcValue& info);
|
||||
|
||||
bool addCallback(const SubscriptionCallbackHelperPtr& helper, const std::string& md5sum, CallbackQueueInterface* queue, int32_t queue_size, const VoidConstPtr& tracked_object, bool allow_concurrent_callbacks);
|
||||
void removeCallback(const SubscriptionCallbackHelperPtr& helper);
|
||||
|
||||
typedef std::map<std::string, std::string> M_string;
|
||||
|
||||
/**
|
||||
* \brief Called to notify that a new message has arrived from a publisher.
|
||||
* Schedules the callback for invokation with the callback queue
|
||||
*/
|
||||
uint32_t handleMessage(const SerializedMessage& m, bool ser, bool nocopy, const boost::shared_ptr<M_string>& connection_header, const PublisherLinkPtr& link);
|
||||
|
||||
const std::string datatype();
|
||||
const std::string md5sum();
|
||||
|
||||
/**
|
||||
* \brief Removes a subscriber from our list
|
||||
*/
|
||||
void removePublisherLink(const PublisherLinkPtr& pub_link);
|
||||
|
||||
const std::string& getName() const { return name_; }
|
||||
uint32_t getNumCallbacks() const { return callbacks_.size(); }
|
||||
uint32_t getNumPublishers();
|
||||
|
||||
// We'll keep a list of these objects, representing in-progress XMLRPC
|
||||
// connections to other nodes.
|
||||
class ROSCPP_DECL PendingConnection : public ASyncXMLRPCConnection
|
||||
{
|
||||
public:
|
||||
PendingConnection(XmlRpc::XmlRpcClient* client, TransportUDPPtr udp_transport, const SubscriptionWPtr& parent, const std::string& remote_uri)
|
||||
: client_(client)
|
||||
, udp_transport_(udp_transport)
|
||||
, parent_(parent)
|
||||
, remote_uri_(remote_uri)
|
||||
{}
|
||||
|
||||
~PendingConnection()
|
||||
{
|
||||
delete client_;
|
||||
}
|
||||
|
||||
XmlRpc::XmlRpcClient* getClient() const { return client_; }
|
||||
TransportUDPPtr getUDPTransport() const { return udp_transport_; }
|
||||
|
||||
virtual void addToDispatch(XmlRpc::XmlRpcDispatch* disp)
|
||||
{
|
||||
disp->addSource(client_, XmlRpc::XmlRpcDispatch::WritableEvent | XmlRpc::XmlRpcDispatch::Exception);
|
||||
}
|
||||
|
||||
virtual void removeFromDispatch(XmlRpc::XmlRpcDispatch* disp)
|
||||
{
|
||||
disp->removeSource(client_);
|
||||
}
|
||||
|
||||
virtual bool check()
|
||||
{
|
||||
SubscriptionPtr parent = parent_.lock();
|
||||
if (!parent)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
XmlRpc::XmlRpcValue result;
|
||||
if (client_->executeCheckDone(result))
|
||||
{
|
||||
parent->pendingConnectionDone(boost::dynamic_pointer_cast<PendingConnection>(shared_from_this()), result);
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
const std::string& getRemoteURI() { return remote_uri_; }
|
||||
|
||||
private:
|
||||
XmlRpc::XmlRpcClient* client_;
|
||||
TransportUDPPtr udp_transport_;
|
||||
SubscriptionWPtr parent_;
|
||||
std::string remote_uri_;
|
||||
};
|
||||
typedef boost::shared_ptr<PendingConnection> PendingConnectionPtr;
|
||||
|
||||
void pendingConnectionDone(const PendingConnectionPtr& pending_conn, XmlRpc::XmlRpcValue& result);
|
||||
|
||||
void getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti);
|
||||
|
||||
void headerReceived(const PublisherLinkPtr& link, const Header& h);
|
||||
|
||||
private:
|
||||
Subscription(const Subscription &); // not copyable
|
||||
Subscription &operator =(const Subscription &); // nor assignable
|
||||
|
||||
void dropAllConnections();
|
||||
|
||||
void addPublisherLink(const PublisherLinkPtr& link);
|
||||
|
||||
struct CallbackInfo
|
||||
{
|
||||
CallbackQueueInterface* callback_queue_;
|
||||
|
||||
// Only used if callback_queue_ is non-NULL (NodeHandle API)
|
||||
SubscriptionCallbackHelperPtr helper_;
|
||||
SubscriptionQueuePtr subscription_queue_;
|
||||
bool has_tracked_object_;
|
||||
VoidConstWPtr tracked_object_;
|
||||
};
|
||||
typedef boost::shared_ptr<CallbackInfo> CallbackInfoPtr;
|
||||
typedef std::vector<CallbackInfoPtr> V_CallbackInfo;
|
||||
|
||||
std::string name_;
|
||||
boost::mutex md5sum_mutex_;
|
||||
std::string md5sum_;
|
||||
std::string datatype_;
|
||||
boost::mutex callbacks_mutex_;
|
||||
V_CallbackInfo callbacks_;
|
||||
uint32_t nonconst_callbacks_;
|
||||
|
||||
bool dropped_;
|
||||
bool shutting_down_;
|
||||
boost::mutex shutdown_mutex_;
|
||||
|
||||
typedef std::set<PendingConnectionPtr> S_PendingConnection;
|
||||
S_PendingConnection pending_connections_;
|
||||
boost::mutex pending_connections_mutex_;
|
||||
|
||||
typedef std::vector<PublisherLinkPtr> V_PublisherLink;
|
||||
V_PublisherLink publisher_links_;
|
||||
boost::mutex publisher_links_mutex_;
|
||||
|
||||
TransportHints transport_hints_;
|
||||
|
||||
StatisticsLogger statistics_;
|
||||
|
||||
struct LatchInfo
|
||||
{
|
||||
SerializedMessage message;
|
||||
PublisherLinkPtr link;
|
||||
boost::shared_ptr<std::map<std::string, std::string> > connection_header;
|
||||
ros::Time receipt_time;
|
||||
};
|
||||
|
||||
typedef std::map<PublisherLinkPtr, LatchInfo> M_PublisherLinkToLatchInfo;
|
||||
M_PublisherLinkToLatchInfo latched_messages_;
|
||||
|
||||
typedef std::vector<std::pair<const std::type_info*, MessageDeserializerPtr> > V_TypeAndDeserializer;
|
||||
V_TypeAndDeserializer cached_deserializers_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
164
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscription_callback_helper.h
vendored
Normal file
164
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscription_callback_helper.h
vendored
Normal file
@@ -0,0 +1,164 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SUBSCRIPTION_CALLBACK_HELPER_H
|
||||
#define ROSCPP_SUBSCRIPTION_CALLBACK_HELPER_H
|
||||
|
||||
#include <typeinfo>
|
||||
|
||||
#include "common.h"
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/parameter_adapter.h"
|
||||
#include "ros/message_traits.h"
|
||||
#include "ros/builtin_message_traits.h"
|
||||
#include "ros/serialization.h"
|
||||
#include "ros/message_event.h"
|
||||
#include <ros/static_assert.h>
|
||||
|
||||
#include <boost/type_traits/add_const.hpp>
|
||||
#include <boost/type_traits/remove_const.hpp>
|
||||
#include <boost/type_traits/remove_reference.hpp>
|
||||
#include <boost/type_traits/is_base_of.hpp>
|
||||
#include <boost/utility/enable_if.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
struct SubscriptionCallbackHelperDeserializeParams
|
||||
{
|
||||
uint8_t* buffer;
|
||||
uint32_t length;
|
||||
boost::shared_ptr<M_string> connection_header;
|
||||
};
|
||||
|
||||
struct ROSCPP_DECL SubscriptionCallbackHelperCallParams
|
||||
{
|
||||
MessageEvent<void const> event;
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Abstract base class used by subscriptions to deal with concrete message types through a common
|
||||
* interface. This is one part of the roscpp API that is \b not fully stable, so overloading this class
|
||||
* is not recommended.
|
||||
*/
|
||||
class ROSCPP_DECL SubscriptionCallbackHelper
|
||||
{
|
||||
public:
|
||||
virtual ~SubscriptionCallbackHelper() {}
|
||||
virtual VoidConstPtr deserialize(const SubscriptionCallbackHelperDeserializeParams&) = 0;
|
||||
virtual void call(SubscriptionCallbackHelperCallParams& params) = 0;
|
||||
virtual const std::type_info& getTypeInfo() = 0;
|
||||
virtual bool isConst() = 0;
|
||||
virtual bool hasHeader() = 0;
|
||||
};
|
||||
typedef boost::shared_ptr<SubscriptionCallbackHelper> SubscriptionCallbackHelperPtr;
|
||||
|
||||
/**
|
||||
* \brief Concrete generic implementation of
|
||||
* SubscriptionCallbackHelper for any normal message type. Use
|
||||
* directly with care, this is mostly for internal use.
|
||||
*/
|
||||
template<typename P, typename Enabled = void>
|
||||
class SubscriptionCallbackHelperT : public SubscriptionCallbackHelper
|
||||
{
|
||||
public:
|
||||
typedef ParameterAdapter<P> Adapter;
|
||||
typedef typename ParameterAdapter<P>::Message NonConstType;
|
||||
typedef typename ParameterAdapter<P>::Event Event;
|
||||
typedef typename boost::add_const<NonConstType>::type ConstType;
|
||||
typedef boost::shared_ptr<NonConstType> NonConstTypePtr;
|
||||
typedef boost::shared_ptr<ConstType> ConstTypePtr;
|
||||
|
||||
static const bool is_const = ParameterAdapter<P>::is_const;
|
||||
|
||||
typedef boost::function<void(typename Adapter::Parameter)> Callback;
|
||||
typedef boost::function<NonConstTypePtr()> CreateFunction;
|
||||
|
||||
SubscriptionCallbackHelperT(const Callback& callback,
|
||||
const CreateFunction& create = DefaultMessageCreator<NonConstType>())
|
||||
: callback_(callback)
|
||||
, create_(create)
|
||||
{ }
|
||||
|
||||
void setCreateFunction(const CreateFunction& create)
|
||||
{
|
||||
create_ = create;
|
||||
}
|
||||
|
||||
virtual bool hasHeader()
|
||||
{
|
||||
return message_traits::hasHeader<typename ParameterAdapter<P>::Message>();
|
||||
}
|
||||
|
||||
virtual VoidConstPtr deserialize(const SubscriptionCallbackHelperDeserializeParams& params)
|
||||
{
|
||||
namespace ser = serialization;
|
||||
|
||||
NonConstTypePtr msg = create_();
|
||||
|
||||
if (!msg)
|
||||
{
|
||||
ROS_DEBUG("Allocation failed for message of type [%s]", getTypeInfo().name());
|
||||
return VoidConstPtr();
|
||||
}
|
||||
|
||||
ser::PreDeserializeParams<NonConstType> predes_params;
|
||||
predes_params.message = msg;
|
||||
predes_params.connection_header = params.connection_header;
|
||||
ser::PreDeserialize<NonConstType>::notify(predes_params);
|
||||
|
||||
ser::IStream stream(params.buffer, params.length);
|
||||
ser::deserialize(stream, *msg);
|
||||
|
||||
return VoidConstPtr(msg);
|
||||
}
|
||||
|
||||
virtual void call(SubscriptionCallbackHelperCallParams& params)
|
||||
{
|
||||
Event event(params.event, create_);
|
||||
callback_(ParameterAdapter<P>::getParameter(event));
|
||||
}
|
||||
|
||||
virtual const std::type_info& getTypeInfo()
|
||||
{
|
||||
return typeid(NonConstType);
|
||||
}
|
||||
|
||||
virtual bool isConst()
|
||||
{
|
||||
return ParameterAdapter<P>::is_const;
|
||||
}
|
||||
|
||||
private:
|
||||
Callback callback_;
|
||||
CreateFunction create_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_SUBSCRIPTION_CALLBACK_HELPER_H
|
||||
95
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscription_queue.h
vendored
Normal file
95
thirdparty/ros/ros_comm/clients/roscpp/include/ros/subscription_queue.h
vendored
Normal file
@@ -0,0 +1,95 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_SUBSCRIPTION_QUEUE_H
|
||||
#define ROSCPP_SUBSCRIPTION_QUEUE_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "common.h"
|
||||
#include "ros/message_event.h"
|
||||
#include "callback_queue_interface.h"
|
||||
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
#include <deque>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class MessageDeserializer;
|
||||
typedef boost::shared_ptr<MessageDeserializer> MessageDeserializerPtr;
|
||||
|
||||
class SubscriptionCallbackHelper;
|
||||
typedef boost::shared_ptr<SubscriptionCallbackHelper> SubscriptionCallbackHelperPtr;
|
||||
|
||||
class ROSCPP_DECL SubscriptionQueue : public CallbackInterface, public boost::enable_shared_from_this<SubscriptionQueue>
|
||||
{
|
||||
private:
|
||||
struct Item
|
||||
{
|
||||
SubscriptionCallbackHelperPtr helper;
|
||||
MessageDeserializerPtr deserializer;
|
||||
|
||||
bool has_tracked_object;
|
||||
VoidConstWPtr tracked_object;
|
||||
|
||||
bool nonconst_need_copy;
|
||||
ros::Time receipt_time;
|
||||
};
|
||||
typedef std::deque<Item> D_Item;
|
||||
|
||||
public:
|
||||
SubscriptionQueue(const std::string& topic, int32_t queue_size, bool allow_concurrent_callbacks);
|
||||
~SubscriptionQueue();
|
||||
|
||||
void push(const SubscriptionCallbackHelperPtr& helper, const MessageDeserializerPtr& deserializer,
|
||||
bool has_tracked_object, const VoidConstWPtr& tracked_object, bool nonconst_need_copy,
|
||||
ros::Time receipt_time = ros::Time(), bool* was_full = 0);
|
||||
void clear();
|
||||
|
||||
virtual CallbackInterface::CallResult call();
|
||||
virtual bool ready();
|
||||
bool full();
|
||||
|
||||
private:
|
||||
bool fullNoLock();
|
||||
std::string topic_;
|
||||
int32_t size_;
|
||||
bool full_;
|
||||
|
||||
boost::mutex queue_mutex_;
|
||||
D_Item queue_;
|
||||
uint32_t queue_size_;
|
||||
bool allow_concurrent_callbacks_;
|
||||
|
||||
boost::recursive_mutex callback_mutex_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_SUBSCRIPTION_QUEUE_H
|
||||
70
thirdparty/ros/ros_comm/clients/roscpp/include/ros/this_node.h
vendored
Normal file
70
thirdparty/ros/ros_comm/clients/roscpp/include/ros/this_node.h
vendored
Normal file
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_THIS_NODE_H
|
||||
#define ROSCPP_THIS_NODE_H
|
||||
|
||||
#include "common.h"
|
||||
#include "forwards.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Contains functions which provide information about this process' ROS node
|
||||
*/
|
||||
namespace this_node
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Returns the name of the current node.
|
||||
*/
|
||||
ROSCPP_DECL const std::string& getName();
|
||||
/**
|
||||
* \brief Returns the namespace of the current node.
|
||||
*/
|
||||
ROSCPP_DECL const std::string& getNamespace();
|
||||
|
||||
/** @brief Get the list of topics advertised by this node
|
||||
*
|
||||
* @param[out] topics The advertised topics
|
||||
*/
|
||||
ROSCPP_DECL void getAdvertisedTopics(V_string& topics);
|
||||
|
||||
/** @brief Get the list of topics subscribed to by this node
|
||||
*
|
||||
* @param[out] The subscribed topics
|
||||
*/
|
||||
ROSCPP_DECL void getSubscribedTopics(V_string& topics);
|
||||
|
||||
} // namespace this_node
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_THIS_NODE_H
|
||||
|
||||
|
||||
130
thirdparty/ros/ros_comm/clients/roscpp/include/ros/timer.h
vendored
Normal file
130
thirdparty/ros/ros_comm/clients/roscpp/include/ros/timer.h
vendored
Normal file
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TIMER_H
|
||||
#define ROSCPP_TIMER_H
|
||||
|
||||
#include "common.h"
|
||||
#include "forwards.h"
|
||||
#include "timer_options.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Manages a timer callback
|
||||
*
|
||||
* A Timer should always be created through a call to NodeHandle::createTimer(), or copied from one
|
||||
* that was. Once all copies of a specific
|
||||
* Timer go out of scope, the callback associated with that handle will stop
|
||||
* being called.
|
||||
*/
|
||||
class ROSCPP_DECL Timer
|
||||
{
|
||||
public:
|
||||
Timer() {}
|
||||
Timer(const Timer& rhs);
|
||||
~Timer();
|
||||
|
||||
/**
|
||||
* \brief Start the timer. Does nothing if the timer is already started.
|
||||
*/
|
||||
void start();
|
||||
/**
|
||||
* \brief Stop the timer. Once this call returns, no more callbacks will be called. Does
|
||||
* nothing if the timer is already stopped.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not the timer has any pending events to call.
|
||||
*/
|
||||
bool hasPending();
|
||||
|
||||
/**
|
||||
* \brief Set the period of this timer
|
||||
* \param reset Whether to reset the timer. If true, timer ignores elapsed time and next cb occurs at now()+period
|
||||
*/
|
||||
void setPeriod(const Duration& period, bool reset=true);
|
||||
|
||||
bool hasStarted() const { return impl_->hasStarted(); }
|
||||
bool isValid() { return impl_ && impl_->isValid(); }
|
||||
operator void*() { return isValid() ? (void*)1 : (void*)0; }
|
||||
|
||||
bool operator<(const Timer& rhs)
|
||||
{
|
||||
return impl_ < rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator==(const Timer& rhs)
|
||||
{
|
||||
return impl_ == rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator!=(const Timer& rhs)
|
||||
{
|
||||
return impl_ != rhs.impl_;
|
||||
}
|
||||
|
||||
private:
|
||||
Timer(const TimerOptions& ops);
|
||||
|
||||
class Impl
|
||||
{
|
||||
public:
|
||||
Impl();
|
||||
~Impl();
|
||||
|
||||
bool hasStarted() const;
|
||||
bool isValid();
|
||||
bool hasPending();
|
||||
void setPeriod(const Duration& period, bool reset=true);
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
|
||||
bool started_;
|
||||
int32_t timer_handle_;
|
||||
|
||||
Duration period_;
|
||||
TimerCallback callback_;
|
||||
CallbackQueueInterface* callback_queue_;
|
||||
VoidConstWPtr tracked_object_;
|
||||
bool has_tracked_object_;
|
||||
bool oneshot_;
|
||||
};
|
||||
typedef boost::shared_ptr<Impl> ImplPtr;
|
||||
typedef boost::weak_ptr<Impl> ImplWPtr;
|
||||
|
||||
ImplPtr impl_;
|
||||
|
||||
friend class NodeHandle;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
590
thirdparty/ros/ros_comm/clients/roscpp/include/ros/timer_manager.h
vendored
Normal file
590
thirdparty/ros/ros_comm/clients/roscpp/include/ros/timer_manager.h
vendored
Normal file
@@ -0,0 +1,590 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TIMER_MANAGER_H
|
||||
#define ROSCPP_TIMER_MANAGER_H
|
||||
|
||||
// check if we might need to include our own backported version boost::condition_variable
|
||||
// in order to use CLOCK_MONOTONIC for the SteadyTimer
|
||||
// the include order here is important!
|
||||
#ifdef BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#include <boost/version.hpp>
|
||||
#if BOOST_VERSION < 106100
|
||||
// use backported version of boost condition variable, see https://svn.boost.org/trac/boost/ticket/6377
|
||||
#include "boost_161_condition_variable.h"
|
||||
#else // Boost version is 1.61 or greater and has the steady clock fixes
|
||||
#include <boost/thread/condition_variable.hpp>
|
||||
#endif
|
||||
#else // !BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#include <boost/thread/condition_variable.hpp>
|
||||
#endif // BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
|
||||
#include "ros/forwards.h"
|
||||
#include "ros/time.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#include <boost/thread/thread.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
#include "ros/assert.h"
|
||||
#include "ros/callback_queue_interface.h"
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
template<class T, class D, class E>
|
||||
class TimerManager
|
||||
{
|
||||
private:
|
||||
struct TimerInfo
|
||||
{
|
||||
int32_t handle;
|
||||
D period;
|
||||
|
||||
boost::function<void(const E&)> callback;
|
||||
CallbackQueueInterface* callback_queue;
|
||||
|
||||
WallDuration last_cb_duration;
|
||||
|
||||
T last_expected;
|
||||
T next_expected;
|
||||
|
||||
T last_real;
|
||||
|
||||
bool removed;
|
||||
|
||||
VoidConstWPtr tracked_object;
|
||||
bool has_tracked_object;
|
||||
|
||||
// TODO: atomicize
|
||||
boost::mutex waiting_mutex;
|
||||
uint32_t waiting_callbacks;
|
||||
|
||||
bool oneshot;
|
||||
|
||||
// debugging info
|
||||
uint32_t total_calls;
|
||||
};
|
||||
typedef boost::shared_ptr<TimerInfo> TimerInfoPtr;
|
||||
typedef boost::weak_ptr<TimerInfo> TimerInfoWPtr;
|
||||
typedef std::vector<TimerInfoPtr> V_TimerInfo;
|
||||
|
||||
typedef std::list<int32_t> L_int32;
|
||||
|
||||
public:
|
||||
TimerManager();
|
||||
~TimerManager();
|
||||
|
||||
int32_t add(const D& period, const boost::function<void(const E&)>& callback, CallbackQueueInterface* callback_queue, const VoidConstPtr& tracked_object, bool oneshot);
|
||||
void remove(int32_t handle);
|
||||
|
||||
bool hasPending(int32_t handle);
|
||||
void setPeriod(int32_t handle, const D& period, bool reset=true);
|
||||
|
||||
static TimerManager& global()
|
||||
{
|
||||
static TimerManager<T, D, E> global;
|
||||
return global;
|
||||
}
|
||||
|
||||
private:
|
||||
void threadFunc();
|
||||
|
||||
bool waitingCompare(int32_t lhs, int32_t rhs);
|
||||
TimerInfoPtr findTimer(int32_t handle);
|
||||
void schedule(const TimerInfoPtr& info);
|
||||
void updateNext(const TimerInfoPtr& info, const T& current_time);
|
||||
|
||||
V_TimerInfo timers_;
|
||||
boost::mutex timers_mutex_;
|
||||
boost::condition_variable timers_cond_;
|
||||
volatile bool new_timer_;
|
||||
|
||||
boost::mutex waiting_mutex_;
|
||||
L_int32 waiting_;
|
||||
|
||||
uint32_t id_counter_;
|
||||
boost::mutex id_mutex_;
|
||||
|
||||
bool thread_started_;
|
||||
|
||||
boost::thread thread_;
|
||||
|
||||
bool quit_;
|
||||
|
||||
class TimerQueueCallback : public CallbackInterface
|
||||
{
|
||||
public:
|
||||
TimerQueueCallback(TimerManager<T, D, E>* parent, const TimerInfoPtr& info, T last_expected, T last_real, T current_expected)
|
||||
: parent_(parent)
|
||||
, info_(info)
|
||||
, last_expected_(last_expected)
|
||||
, last_real_(last_real)
|
||||
, current_expected_(current_expected)
|
||||
, called_(false)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(info->waiting_mutex);
|
||||
++info->waiting_callbacks;
|
||||
}
|
||||
|
||||
~TimerQueueCallback()
|
||||
{
|
||||
TimerInfoPtr info = info_.lock();
|
||||
if (info)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(info->waiting_mutex);
|
||||
--info->waiting_callbacks;
|
||||
}
|
||||
}
|
||||
|
||||
CallResult call()
|
||||
{
|
||||
TimerInfoPtr info = info_.lock();
|
||||
if (!info)
|
||||
{
|
||||
return Invalid;
|
||||
}
|
||||
|
||||
{
|
||||
++info->total_calls;
|
||||
called_ = true;
|
||||
|
||||
VoidConstPtr tracked;
|
||||
if (info->has_tracked_object)
|
||||
{
|
||||
tracked = info->tracked_object.lock();
|
||||
if (!tracked)
|
||||
{
|
||||
return Invalid;
|
||||
}
|
||||
}
|
||||
|
||||
E event;
|
||||
event.last_expected = last_expected_;
|
||||
event.last_real = last_real_;
|
||||
event.current_expected = current_expected_;
|
||||
event.current_real = T::now();
|
||||
event.profile.last_duration = info->last_cb_duration;
|
||||
|
||||
SteadyTime cb_start = SteadyTime::now();
|
||||
info->callback(event);
|
||||
SteadyTime cb_end = SteadyTime::now();
|
||||
info->last_cb_duration = cb_end - cb_start;
|
||||
|
||||
info->last_real = event.current_real;
|
||||
|
||||
parent_->schedule(info);
|
||||
}
|
||||
|
||||
return Success;
|
||||
}
|
||||
|
||||
private:
|
||||
TimerManager<T, D, E>* parent_;
|
||||
TimerInfoWPtr info_;
|
||||
T last_expected_;
|
||||
T last_real_;
|
||||
T current_expected_;
|
||||
|
||||
bool called_;
|
||||
};
|
||||
};
|
||||
|
||||
template<class T, class D, class E>
|
||||
TimerManager<T, D, E>::TimerManager() :
|
||||
new_timer_(false), id_counter_(0), thread_started_(false), quit_(false)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
TimerManager<T, D, E>::~TimerManager()
|
||||
{
|
||||
quit_ = true;
|
||||
{
|
||||
boost::mutex::scoped_lock lock(timers_mutex_);
|
||||
timers_cond_.notify_all();
|
||||
}
|
||||
if (thread_started_)
|
||||
{
|
||||
thread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
bool TimerManager<T, D, E>::waitingCompare(int32_t lhs, int32_t rhs)
|
||||
{
|
||||
TimerInfoPtr infol = findTimer(lhs);
|
||||
TimerInfoPtr infor = findTimer(rhs);
|
||||
if (!infol || !infor)
|
||||
{
|
||||
return infol < infor;
|
||||
}
|
||||
|
||||
return infol->next_expected < infor->next_expected;
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
typename TimerManager<T, D, E>::TimerInfoPtr TimerManager<T, D, E>::findTimer(int32_t handle)
|
||||
{
|
||||
typename V_TimerInfo::iterator it = timers_.begin();
|
||||
typename V_TimerInfo::iterator end = timers_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
if ((*it)->handle == handle)
|
||||
{
|
||||
return *it;
|
||||
}
|
||||
}
|
||||
|
||||
return TimerInfoPtr();
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
bool TimerManager<T, D, E>::hasPending(int32_t handle)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(timers_mutex_);
|
||||
TimerInfoPtr info = findTimer(handle);
|
||||
|
||||
if (!info)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (info->has_tracked_object)
|
||||
{
|
||||
VoidConstPtr tracked = info->tracked_object.lock();
|
||||
if (!tracked)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
boost::mutex::scoped_lock lock2(info->waiting_mutex);
|
||||
return info->next_expected <= T::now() || info->waiting_callbacks != 0;
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
int32_t TimerManager<T, D, E>::add(const D& period, const boost::function<void(const E&)>& callback, CallbackQueueInterface* callback_queue,
|
||||
const VoidConstPtr& tracked_object, bool oneshot)
|
||||
{
|
||||
TimerInfoPtr info(boost::make_shared<TimerInfo>());
|
||||
info->period = period;
|
||||
info->callback = callback;
|
||||
info->callback_queue = callback_queue;
|
||||
info->last_expected = T::now();
|
||||
info->next_expected = info->last_expected + period;
|
||||
info->removed = false;
|
||||
info->has_tracked_object = false;
|
||||
info->waiting_callbacks = 0;
|
||||
info->total_calls = 0;
|
||||
info->oneshot = oneshot;
|
||||
if (tracked_object)
|
||||
{
|
||||
info->tracked_object = tracked_object;
|
||||
info->has_tracked_object = true;
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(id_mutex_);
|
||||
info->handle = id_counter_++;
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(timers_mutex_);
|
||||
timers_.push_back(info);
|
||||
|
||||
if (!thread_started_)
|
||||
{
|
||||
thread_ = boost::thread(boost::bind(&TimerManager::threadFunc, this));
|
||||
thread_started_ = true;
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(waiting_mutex_);
|
||||
waiting_.push_back(info->handle);
|
||||
waiting_.sort(boost::bind(&TimerManager::waitingCompare, this, _1, _2));
|
||||
}
|
||||
|
||||
new_timer_ = true;
|
||||
timers_cond_.notify_all();
|
||||
}
|
||||
|
||||
return info->handle;
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
void TimerManager<T, D, E>::remove(int32_t handle)
|
||||
{
|
||||
CallbackQueueInterface* callback_queue = 0;
|
||||
uint64_t remove_id = 0;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(timers_mutex_);
|
||||
|
||||
typename V_TimerInfo::iterator it = timers_.begin();
|
||||
typename V_TimerInfo::iterator end = timers_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const TimerInfoPtr& info = *it;
|
||||
if (info->handle == handle)
|
||||
{
|
||||
info->removed = true;
|
||||
callback_queue = info->callback_queue;
|
||||
remove_id = (uint64_t)info.get();
|
||||
timers_.erase(it);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock2(waiting_mutex_);
|
||||
// Remove from the waiting list if it's in it
|
||||
L_int32::iterator it = std::find(waiting_.begin(), waiting_.end(), handle);
|
||||
if (it != waiting_.end())
|
||||
{
|
||||
waiting_.erase(it);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (callback_queue)
|
||||
{
|
||||
callback_queue->removeByID(remove_id);
|
||||
}
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
void TimerManager<T, D, E>::schedule(const TimerInfoPtr& info)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(timers_mutex_);
|
||||
|
||||
if (info->removed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
updateNext(info, T::now());
|
||||
{
|
||||
boost::mutex::scoped_lock lock(waiting_mutex_);
|
||||
|
||||
waiting_.push_back(info->handle);
|
||||
// waitingCompare requires a lock on the timers_mutex_
|
||||
waiting_.sort(boost::bind(&TimerManager::waitingCompare, this, _1, _2));
|
||||
}
|
||||
|
||||
new_timer_ = true;
|
||||
timers_cond_.notify_one();
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
void TimerManager<T, D, E>::updateNext(const TimerInfoPtr& info, const T& current_time)
|
||||
{
|
||||
if (info->oneshot)
|
||||
{
|
||||
info->next_expected = T(INT_MAX, 999999999);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Protect against someone having called setPeriod()
|
||||
// If the next expected time is already past the current time
|
||||
// don't update it
|
||||
if (info->next_expected <= current_time)
|
||||
{
|
||||
info->last_expected = info->next_expected;
|
||||
info->next_expected += info->period;
|
||||
}
|
||||
|
||||
// detect time jumping forward, as well as callbacks that are too slow
|
||||
if (info->next_expected + info->period < current_time)
|
||||
{
|
||||
ROS_DEBUG("Time jumped forward by [%f] for timer of period [%f], resetting timer (current=%f, next_expected=%f)", (current_time - info->next_expected).toSec(), info->period.toSec(), current_time.toSec(), info->next_expected.toSec());
|
||||
info->next_expected = current_time;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
void TimerManager<T, D, E>::setPeriod(int32_t handle, const D& period, bool reset)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(timers_mutex_);
|
||||
TimerInfoPtr info = findTimer(handle);
|
||||
|
||||
if (!info)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(waiting_mutex_);
|
||||
|
||||
if(reset)
|
||||
{
|
||||
info->next_expected = T::now() + period;
|
||||
}
|
||||
|
||||
// else if some time has elapsed since last cb (called outside of cb)
|
||||
else if( (T::now() - info->last_real) < info->period)
|
||||
{
|
||||
// if elapsed time is greater than the new period
|
||||
// do the callback now
|
||||
if( (T::now() - info->last_real) > period)
|
||||
{
|
||||
info->next_expected = T::now();
|
||||
}
|
||||
|
||||
// else, account for elapsed time by using last_real+period
|
||||
else
|
||||
{
|
||||
info->next_expected = info->last_real + period;
|
||||
}
|
||||
}
|
||||
|
||||
// Else if called in a callback, last_real has not been updated yet => (now - last_real) > period
|
||||
// In this case, let next_expected be updated only in updateNext
|
||||
|
||||
info->period = period;
|
||||
waiting_.sort(boost::bind(&TimerManager::waitingCompare, this, _1, _2));
|
||||
}
|
||||
|
||||
new_timer_ = true;
|
||||
timers_cond_.notify_one();
|
||||
}
|
||||
|
||||
template<class T, class D, class E>
|
||||
void TimerManager<T, D, E>::threadFunc()
|
||||
{
|
||||
T current;
|
||||
while (!quit_)
|
||||
{
|
||||
T sleep_end;
|
||||
|
||||
boost::mutex::scoped_lock lock(timers_mutex_);
|
||||
|
||||
// detect time jumping backwards
|
||||
if (T::now() < current)
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Time jumped backward, resetting timers");
|
||||
|
||||
current = T::now();
|
||||
|
||||
typename V_TimerInfo::iterator it = timers_.begin();
|
||||
typename V_TimerInfo::iterator end = timers_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const TimerInfoPtr& info = *it;
|
||||
|
||||
// Timer may have been added after the time jump, so also check if time has jumped past its last call time
|
||||
if (current < info->last_expected)
|
||||
{
|
||||
info->last_expected = current;
|
||||
info->next_expected = current + info->period;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
current = T::now();
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock waitlock(waiting_mutex_);
|
||||
|
||||
if (waiting_.empty())
|
||||
{
|
||||
sleep_end = current + D(0.1);
|
||||
}
|
||||
else
|
||||
{
|
||||
TimerInfoPtr info = findTimer(waiting_.front());
|
||||
|
||||
while (!waiting_.empty() && info && info->next_expected <= current)
|
||||
{
|
||||
current = T::now();
|
||||
|
||||
//ROS_DEBUG("Scheduling timer callback for timer [%d] of period [%f], [%f] off expected", info->handle, info->period.toSec(), (current - info->next_expected).toSec());
|
||||
CallbackInterfacePtr cb(boost::make_shared<TimerQueueCallback>(this, info, info->last_expected, info->last_real, info->next_expected));
|
||||
info->callback_queue->addCallback(cb, (uint64_t)info.get());
|
||||
|
||||
waiting_.pop_front();
|
||||
|
||||
if (waiting_.empty())
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
info = findTimer(waiting_.front());
|
||||
}
|
||||
|
||||
if (info)
|
||||
{
|
||||
sleep_end = info->next_expected;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
while (!new_timer_ && T::now() < sleep_end && !quit_)
|
||||
{
|
||||
// detect backwards jumps in time
|
||||
|
||||
if (T::now() < current)
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Time jumped backwards, breaking out of sleep");
|
||||
break;
|
||||
}
|
||||
|
||||
current = T::now();
|
||||
|
||||
if (current >= sleep_end)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// If we're on simulation time we need to check now() against sleep_end more often than on system time,
|
||||
// since simulation time may be running faster than real time.
|
||||
if (!T::isSystemTime())
|
||||
{
|
||||
timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(1));
|
||||
}
|
||||
else
|
||||
{
|
||||
// On system time we can simply sleep for the rest of the wait time, since anything else requiring processing will
|
||||
// signal the condition variable
|
||||
int64_t remaining_time = std::max<int64_t>((sleep_end - current).toSec() * 1000.0f, 1);
|
||||
timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time));
|
||||
}
|
||||
}
|
||||
|
||||
new_timer_ = false;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
85
thirdparty/ros/ros_comm/clients/roscpp/include/ros/timer_options.h
vendored
Normal file
85
thirdparty/ros/ros_comm/clients/roscpp/include/ros/timer_options.h
vendored
Normal file
@@ -0,0 +1,85 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TIMER_OPTIONS_H
|
||||
#define ROSCPP_TIMER_OPTIONS_H
|
||||
|
||||
#include "common.h"
|
||||
#include "ros/forwards.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Encapsulates all options available for starting a timer
|
||||
*/
|
||||
struct ROSCPP_DECL TimerOptions
|
||||
{
|
||||
TimerOptions()
|
||||
: period(0.1)
|
||||
, callback_queue(0)
|
||||
, oneshot(false)
|
||||
, autostart(true)
|
||||
{ }
|
||||
|
||||
/*
|
||||
* \brief Constructor
|
||||
* \param
|
||||
*/
|
||||
TimerOptions(Duration _period, const TimerCallback& _callback,
|
||||
CallbackQueueInterface* _queue, bool oneshot = false, bool autostart = true)
|
||||
: period(_period)
|
||||
, callback(_callback)
|
||||
, callback_queue(_queue)
|
||||
, oneshot(oneshot)
|
||||
, autostart(autostart)
|
||||
{ }
|
||||
|
||||
Duration period; ///< The period to call the callback at
|
||||
TimerCallback callback; ///< The callback to call
|
||||
|
||||
CallbackQueueInterface* callback_queue; ///< Queue to add callbacks to. If NULL, the global callback queue will be used
|
||||
|
||||
/**
|
||||
* A shared pointer to an object to track for these callbacks. If set, the a weak_ptr will be created to this object,
|
||||
* and if the reference count goes to 0 the subscriber callbacks will not get called.
|
||||
*
|
||||
* \note Note that setting this will cause a new reference to be added to the object before the
|
||||
* callback, and for it to go out of scope (and potentially be deleted) in the code path (and therefore
|
||||
* thread) that the callback is invoked from.
|
||||
*/
|
||||
VoidConstPtr tracked_object;
|
||||
|
||||
bool oneshot;
|
||||
bool autostart;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
143
thirdparty/ros/ros_comm/clients/roscpp/include/ros/topic.h
vendored
Normal file
143
thirdparty/ros/ros_comm/clients/roscpp/include/ros/topic.h
vendored
Normal file
@@ -0,0 +1,143 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TOPIC_H
|
||||
#define ROSCPP_TOPIC_H
|
||||
|
||||
#include "common.h"
|
||||
#include "node_handle.h"
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
namespace topic
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Internal method, do not use
|
||||
*/
|
||||
ROSCPP_DECL void waitForMessageImpl(SubscribeOptions& ops, const boost::function<bool(void)>& ready_pred, NodeHandle& nh, ros::Duration timeout);
|
||||
|
||||
template<class M>
|
||||
class SubscribeHelper
|
||||
{
|
||||
public:
|
||||
typedef boost::shared_ptr<M const> MConstPtr;
|
||||
void callback(const MConstPtr& message)
|
||||
{
|
||||
message_ = message;
|
||||
}
|
||||
|
||||
bool hasMessage()
|
||||
{
|
||||
return static_cast<bool>(message_);
|
||||
}
|
||||
|
||||
MConstPtr getMessage()
|
||||
{
|
||||
return message_;
|
||||
}
|
||||
|
||||
private:
|
||||
MConstPtr message_;
|
||||
};
|
||||
|
||||
/**
|
||||
* \brief Wait for a single message to arrive on a topic, with timeout
|
||||
*
|
||||
* \param M <template> The message type
|
||||
* \param topic The topic to subscribe on
|
||||
* \param nh The NodeHandle to use to do the subscription
|
||||
* \param timeout The amount of time to wait before returning if no message is received
|
||||
* \return The message. Empty boost::shared_ptr if waitForMessage is interrupted by the node shutting down
|
||||
*/
|
||||
template<class M>
|
||||
boost::shared_ptr<M const> waitForMessage(const std::string& topic, NodeHandle& nh, ros::Duration timeout)
|
||||
{
|
||||
SubscribeHelper<M> helper;
|
||||
SubscribeOptions ops;
|
||||
ops.template init<M>(topic, 1, boost::bind(&SubscribeHelper<M>::callback, &helper, _1));
|
||||
|
||||
waitForMessageImpl(ops, boost::bind(&SubscribeHelper<M>::hasMessage, &helper), nh, timeout);
|
||||
|
||||
return helper.getMessage();
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Wait for a single message to arrive on a topic
|
||||
*
|
||||
* \param M <template> The message type
|
||||
* \param topic The topic to subscribe on
|
||||
* \return The message. Empty boost::shared_ptr if waitForMessage is interrupted by the node shutting down
|
||||
*/
|
||||
template<class M>
|
||||
boost::shared_ptr<M const> waitForMessage(const std::string& topic)
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
return waitForMessage<M>(topic, nh, ros::Duration());
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Wait for a single message to arrive on a topic, with timeout
|
||||
*
|
||||
* \param M <template> The message type
|
||||
* \param topic The topic to subscribe on
|
||||
* \param timeout The amount of time to wait before returning if no message is received
|
||||
* \return The message. Empty boost::shared_ptr if waitForMessage is interrupted by the node shutting down
|
||||
*/
|
||||
template<class M>
|
||||
boost::shared_ptr<M const> waitForMessage(const std::string& topic, ros::Duration timeout)
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
return waitForMessage<M>(topic, nh, timeout);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Wait for a single message to arrive on a topic
|
||||
*
|
||||
* \param M <template> The message type
|
||||
* \param topic The topic to subscribe on
|
||||
* \param nh The NodeHandle to use to do the subscription
|
||||
* \return The message. Empty boost::shared_ptr if waitForMessage is interrupted by the node shutting down
|
||||
*/
|
||||
template<class M>
|
||||
boost::shared_ptr<M const> waitForMessage(const std::string& topic, ros::NodeHandle& nh)
|
||||
{
|
||||
return waitForMessage<M>(topic, nh, ros::Duration());
|
||||
}
|
||||
|
||||
} // namespace topic
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_TOPIC_H
|
||||
239
thirdparty/ros/ros_comm/clients/roscpp/include/ros/topic_manager.h
vendored
Normal file
239
thirdparty/ros/ros_comm/clients/roscpp/include/ros/topic_manager.h
vendored
Normal file
@@ -0,0 +1,239 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TOPIC_MANAGER_H
|
||||
#define ROSCPP_TOPIC_MANAGER_H
|
||||
|
||||
#include "forwards.h"
|
||||
#include "common.h"
|
||||
#include "ros/serialization.h"
|
||||
#include "rosout_appender.h"
|
||||
|
||||
#include "xmlrpcpp/XmlRpcValue.h"
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class Message;
|
||||
struct SubscribeOptions;
|
||||
struct AdvertiseOptions;
|
||||
|
||||
class TopicManager;
|
||||
typedef boost::shared_ptr<TopicManager> TopicManagerPtr;
|
||||
|
||||
class PollManager;
|
||||
typedef boost::shared_ptr<PollManager> PollManagerPtr;
|
||||
|
||||
class XMLRPCManager;
|
||||
typedef boost::shared_ptr<XMLRPCManager> XMLRPCManagerPtr;
|
||||
|
||||
class ConnectionManager;
|
||||
typedef boost::shared_ptr<ConnectionManager> ConnectionManagerPtr;
|
||||
|
||||
class SubscriptionCallbackHelper;
|
||||
typedef boost::shared_ptr<SubscriptionCallbackHelper> SubscriptionCallbackHelperPtr;
|
||||
|
||||
class ROSCPP_DECL TopicManager
|
||||
{
|
||||
public:
|
||||
static const TopicManagerPtr& instance();
|
||||
|
||||
TopicManager();
|
||||
~TopicManager();
|
||||
|
||||
void start();
|
||||
void shutdown();
|
||||
|
||||
bool subscribe(const SubscribeOptions& ops);
|
||||
bool unsubscribe(const std::string &_topic, const SubscriptionCallbackHelperPtr& helper);
|
||||
|
||||
bool advertise(const AdvertiseOptions& ops, const SubscriberCallbacksPtr& callbacks);
|
||||
bool unadvertise(const std::string &topic, const SubscriberCallbacksPtr& callbacks);
|
||||
|
||||
/** @brief Get the list of topics advertised by this node
|
||||
*
|
||||
* @param[out] topics The advertised topics
|
||||
*/
|
||||
void getAdvertisedTopics(V_string& topics);
|
||||
|
||||
/** @brief Get the list of topics subscribed to by this node
|
||||
*
|
||||
* @param[out] The subscribed topics
|
||||
*/
|
||||
void getSubscribedTopics(V_string& topics);
|
||||
|
||||
/** @brief Lookup an advertised topic.
|
||||
*
|
||||
* This method iterates over advertised_topics, looking for one with name
|
||||
* matching the given topic name. The advertised_topics_mutex is locked
|
||||
* during this search. This method is only used internally.
|
||||
*
|
||||
* @param topic The topic name to look for.
|
||||
*
|
||||
* @returns Pointer to the matching Publication, NULL if none is found.
|
||||
*/
|
||||
PublicationPtr lookupPublication(const std::string& topic);
|
||||
|
||||
/** @brief Return the number of subscribers a node has for a particular topic:
|
||||
*
|
||||
* @param _topic The topic name to check
|
||||
*
|
||||
* @return number of subscribers
|
||||
*/
|
||||
size_t getNumSubscribers(const std::string &_topic);
|
||||
size_t getNumSubscriptions();
|
||||
|
||||
/**
|
||||
* \brief Return the number of publishers connected to this node on a particular topic
|
||||
*
|
||||
* \param _topic the topic name to check
|
||||
* \return the number of subscribers
|
||||
*/
|
||||
size_t getNumPublishers(const std::string &_topic);
|
||||
|
||||
template<typename M>
|
||||
void publish(const std::string& topic, const M& message)
|
||||
{
|
||||
using namespace serialization;
|
||||
|
||||
SerializedMessage m;
|
||||
publish(topic, boost::bind(serializeMessage<M>, boost::ref(message)), m);
|
||||
}
|
||||
|
||||
void publish(const std::string &_topic, const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m);
|
||||
|
||||
void incrementSequence(const std::string &_topic);
|
||||
bool isLatched(const std::string& topic);
|
||||
|
||||
private:
|
||||
/** if it finds a pre-existing subscription to the same topic and of the
|
||||
* same message type, it appends the Functor to the callback vector for
|
||||
* that subscription. otherwise, it returns false, indicating that a new
|
||||
* subscription needs to be created.
|
||||
*/
|
||||
bool addSubCallback(const SubscribeOptions& ops);
|
||||
|
||||
/** @brief Request a topic
|
||||
*
|
||||
* Negotiate a subscriber connection on a topic.
|
||||
*
|
||||
* @param topic The topic of interest.
|
||||
* @param protos List of transport protocols, in preference order
|
||||
* @param ret Return value
|
||||
*
|
||||
* @return true on success, false otherwise
|
||||
*
|
||||
* @todo Consider making this private
|
||||
*/
|
||||
bool requestTopic(const std::string &topic, XmlRpc::XmlRpcValue &protos, XmlRpc::XmlRpcValue &ret);
|
||||
|
||||
// Must lock the advertised topics mutex before calling this function
|
||||
bool isTopicAdvertised(const std::string& topic);
|
||||
|
||||
bool registerSubscriber(const SubscriptionPtr& s, const std::string& datatype);
|
||||
bool unregisterSubscriber(const std::string& topic);
|
||||
bool unregisterPublisher(const std::string& topic);
|
||||
|
||||
PublicationPtr lookupPublicationWithoutLock(const std::string &topic);
|
||||
|
||||
void processPublishQueues();
|
||||
|
||||
/** @brief Compute the statistics for the node's connectivity
|
||||
*
|
||||
* This is the implementation of the xml-rpc getBusStats function;
|
||||
* it populates the XmlRpcValue object sent to it with various statistics
|
||||
* about the node's connectivity, bandwidth utilization, etc.
|
||||
*/
|
||||
void getBusStats(XmlRpc::XmlRpcValue &stats);
|
||||
|
||||
/** @brief Compute the info for the node's connectivity
|
||||
*
|
||||
* This is the implementation of the xml-rpc getBusInfo function;
|
||||
* it populates the XmlRpcValue object sent to it with various info
|
||||
* about the node's connectivity.
|
||||
*/
|
||||
void getBusInfo(XmlRpc::XmlRpcValue &info);
|
||||
|
||||
/** @brief Return the list of subcriptions for the node
|
||||
*
|
||||
* This is the implementation of the xml-rpc getSubscriptions
|
||||
* function; it populates the XmlRpcValue object sent to it with the
|
||||
* list of subscribed topics and their datatypes.
|
||||
*/
|
||||
void getSubscriptions(XmlRpc::XmlRpcValue &subscriptions);
|
||||
|
||||
/** @brief Return the list of advertised topics for the node
|
||||
*
|
||||
* This is the implementation of the xml-rpc getPublications
|
||||
* function; it populates the XmlRpcValue object sent to it with the
|
||||
* list of advertised topics and their datatypes.
|
||||
*/
|
||||
void getPublications(XmlRpc::XmlRpcValue &publications);
|
||||
|
||||
/** @brief Update local publisher lists.
|
||||
*
|
||||
* Use this method to update address information for publishers on a
|
||||
* given topic.
|
||||
*
|
||||
* @param topic The topic of interest
|
||||
* @param pubs The list of publishers to update.
|
||||
*
|
||||
* @return true on success, false otherwise.
|
||||
*/
|
||||
bool pubUpdate(const std::string &topic, const std::vector<std::string> &pubs);
|
||||
|
||||
void pubUpdateCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result);
|
||||
void requestTopicCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result);
|
||||
void getBusStatsCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result);
|
||||
void getBusInfoCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result);
|
||||
void getSubscriptionsCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result);
|
||||
void getPublicationsCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result);
|
||||
|
||||
bool isShuttingDown() { return shutting_down_; }
|
||||
|
||||
boost::mutex subs_mutex_;
|
||||
L_Subscription subscriptions_;
|
||||
|
||||
boost::recursive_mutex advertised_topics_mutex_;
|
||||
V_Publication advertised_topics_;
|
||||
std::list<std::string> advertised_topic_names_;
|
||||
boost::mutex advertised_topic_names_mutex_;
|
||||
|
||||
volatile bool shutting_down_;
|
||||
boost::mutex shutting_down_mutex_;
|
||||
|
||||
PollManagerPtr poll_manager_;
|
||||
ConnectionManagerPtr connection_manager_;
|
||||
XMLRPCManagerPtr xmlrpc_manager_;
|
||||
};
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_TOPIC_MANAGER_H
|
||||
156
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport/transport.h
vendored
Normal file
156
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport/transport.h
vendored
Normal file
@@ -0,0 +1,156 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TRANSPORT_H
|
||||
#define ROSCPP_TRANSPORT_H
|
||||
|
||||
#include <ros/types.h>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class Transport;
|
||||
typedef boost::shared_ptr<Transport> TransportPtr;
|
||||
|
||||
class Header;
|
||||
|
||||
/**
|
||||
* \brief Abstract base class that allows abstraction of the transport type, eg. TCP, shared memory, UDP...
|
||||
*/
|
||||
class Transport : public boost::enable_shared_from_this<Transport>
|
||||
{
|
||||
public:
|
||||
Transport();
|
||||
virtual ~Transport() {}
|
||||
|
||||
/**
|
||||
* \brief Read a number of bytes into the supplied buffer. Not guaranteed to actually read that number of bytes.
|
||||
* \param buffer Buffer to read from
|
||||
* \param size Size, in bytes, to read
|
||||
* \return The number of bytes actually read, or -1 if there was an error
|
||||
*/
|
||||
virtual int32_t read(uint8_t* buffer, uint32_t size) = 0;
|
||||
/**
|
||||
* \brief Write a number of bytes from the supplied buffer. Not guaranteed to actually write that number of bytes.
|
||||
* \param buffer Buffer to write from
|
||||
* \param size Size, in bytes, to write
|
||||
* \return The number of bytes actually written, or -1 if there was an error
|
||||
*/
|
||||
virtual int32_t write(uint8_t* buffer, uint32_t size) = 0;
|
||||
|
||||
/**
|
||||
* \brief Enable writing on this transport. Allows derived classes to, for example, enable write polling for asynchronous sockets
|
||||
*/
|
||||
virtual void enableWrite() = 0;
|
||||
/**
|
||||
* \brief Disable writing on this transport. Allows derived classes to, for example, disable write polling for asynchronous sockets
|
||||
*/
|
||||
virtual void disableWrite() = 0;
|
||||
|
||||
/**
|
||||
* \brief Enable reading on this transport. Allows derived classes to, for example, enable read polling for asynchronous sockets
|
||||
*/
|
||||
virtual void enableRead() = 0;
|
||||
/**
|
||||
* \brief Disable reading on this transport. Allows derived classes to, for example, disable read polling for asynchronous sockets
|
||||
*/
|
||||
virtual void disableRead() = 0;
|
||||
|
||||
/**
|
||||
* \brief Close this transport. Once this call has returned, writing on this transport should always return an error.
|
||||
*/
|
||||
virtual void close() = 0;
|
||||
|
||||
/**
|
||||
* \brief Return a string that details the type of transport (Eg. TCPROS)
|
||||
* \return The stringified transport type
|
||||
*/
|
||||
virtual const char* getType() = 0;
|
||||
|
||||
typedef boost::function<void(const TransportPtr&)> Callback;
|
||||
/**
|
||||
* \brief Set the function to call when this transport has disconnected, either through a call to close(). Or a disconnect from the remote host.
|
||||
*/
|
||||
void setDisconnectCallback(const Callback& cb) { disconnect_cb_ = cb; }
|
||||
/**
|
||||
* \brief Set the function to call when there is data available to be read by this transport
|
||||
*/
|
||||
void setReadCallback(const Callback& cb) { read_cb_ = cb; }
|
||||
/**
|
||||
* \brief Set the function to call when there is space available to write on this transport
|
||||
*/
|
||||
void setWriteCallback(const Callback& cb) { write_cb_ = cb; }
|
||||
|
||||
/**
|
||||
* \brief Returns a string description of both the type of transport and who the transport is connected to
|
||||
*/
|
||||
virtual std::string getTransportInfo() = 0;
|
||||
|
||||
/**
|
||||
* \brief Returns a boolean to indicate if the transport mechanism is reliable or not
|
||||
*/
|
||||
virtual bool requiresHeader() {return true;}
|
||||
|
||||
/**
|
||||
* \brief Provides an opportunity for transport-specific options to come in through the header
|
||||
*/
|
||||
virtual void parseHeader(const Header& header) { (void)header; }
|
||||
|
||||
protected:
|
||||
Callback disconnect_cb_;
|
||||
Callback read_cb_;
|
||||
Callback write_cb_;
|
||||
|
||||
/**
|
||||
* \brief returns true if the transport is allowed to connect to the host passed to it.
|
||||
*/
|
||||
bool isHostAllowed(const std::string &host) const;
|
||||
|
||||
/**
|
||||
* \brief returns true if this transport is only allowed to talk to localhost
|
||||
*/
|
||||
bool isOnlyLocalhostAllowed() const { return only_localhost_allowed_; }
|
||||
|
||||
private:
|
||||
bool only_localhost_allowed_;
|
||||
std::vector<std::string> allowed_hosts_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_TRANSPORT_H
|
||||
171
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport/transport_tcp.h
vendored
Normal file
171
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport/transport_tcp.h
vendored
Normal file
@@ -0,0 +1,171 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TRANSPORT_TCP_H
|
||||
#define ROSCPP_TRANSPORT_TCP_H
|
||||
|
||||
#include <ros/types.h>
|
||||
#include <ros/transport/transport.h>
|
||||
|
||||
#include <boost/thread/recursive_mutex.hpp>
|
||||
#include "ros/io.h"
|
||||
#include <ros/common.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class TransportTCP;
|
||||
typedef boost::shared_ptr<TransportTCP> TransportTCPPtr;
|
||||
|
||||
class PollSet;
|
||||
|
||||
/**
|
||||
* \brief TCPROS transport
|
||||
*/
|
||||
class ROSCPP_DECL TransportTCP : public Transport
|
||||
{
|
||||
public:
|
||||
static bool s_use_keepalive_;
|
||||
static bool s_use_ipv6_;
|
||||
|
||||
public:
|
||||
enum Flags
|
||||
{
|
||||
SYNCHRONOUS = 1<<0,
|
||||
};
|
||||
|
||||
TransportTCP(PollSet* poll_set, int flags = 0);
|
||||
virtual ~TransportTCP();
|
||||
|
||||
/**
|
||||
* \brief Connect to a remote host.
|
||||
* \param host The hostname/IP to connect to
|
||||
* \param port The port to connect to
|
||||
* \return Whether or not the connection was successful
|
||||
*/
|
||||
bool connect(const std::string& host, int port);
|
||||
|
||||
/**
|
||||
* \brief Returns the URI of the remote host
|
||||
*/
|
||||
std::string getClientURI();
|
||||
|
||||
typedef boost::function<void(const TransportTCPPtr&)> AcceptCallback;
|
||||
/**
|
||||
* \brief Start a server socket and listen on a port
|
||||
* \param port The port to listen on
|
||||
* \param backlog defines the maximum length for the queue of pending connections. Identical to the backlog parameter to the ::listen function
|
||||
* \param accept_cb The function to call when a client socket has connected
|
||||
*/
|
||||
bool listen(int port, int backlog, const AcceptCallback& accept_cb);
|
||||
/**
|
||||
* \brief Accept a connection on a server socket. Blocks until a connection is available
|
||||
*/
|
||||
TransportTCPPtr accept();
|
||||
/**
|
||||
* \brief Returns the port this transport is listening on
|
||||
*/
|
||||
int getServerPort() { return server_port_; }
|
||||
int getLocalPort() { return local_port_; }
|
||||
|
||||
void setNoDelay(bool nodelay);
|
||||
void setKeepAlive(bool use, uint32_t idle, uint32_t interval, uint32_t count);
|
||||
|
||||
const std::string& getConnectedHost() { return connected_host_; }
|
||||
int getConnectedPort() { return connected_port_; }
|
||||
|
||||
// overrides from Transport
|
||||
virtual int32_t read(uint8_t* buffer, uint32_t size);
|
||||
virtual int32_t write(uint8_t* buffer, uint32_t size);
|
||||
|
||||
virtual void enableWrite();
|
||||
virtual void disableWrite();
|
||||
virtual void enableRead();
|
||||
virtual void disableRead();
|
||||
|
||||
virtual void close();
|
||||
|
||||
virtual std::string getTransportInfo();
|
||||
|
||||
virtual void parseHeader(const Header& header);
|
||||
|
||||
virtual const char* getType() { return "TCPROS"; }
|
||||
|
||||
private:
|
||||
/**
|
||||
* \brief Initializes the assigned socket -- sets it to non-blocking and enables reading
|
||||
*/
|
||||
bool initializeSocket();
|
||||
|
||||
bool setNonBlocking();
|
||||
|
||||
/**
|
||||
* \brief Set the socket to be used by this transport
|
||||
* \param sock A valid TCP socket
|
||||
* \return Whether setting the socket was successful
|
||||
*/
|
||||
bool setSocket(int sock);
|
||||
|
||||
void socketUpdate(int events);
|
||||
|
||||
socket_fd_t sock_;
|
||||
bool closed_;
|
||||
boost::recursive_mutex close_mutex_;
|
||||
|
||||
bool expecting_read_;
|
||||
bool expecting_write_;
|
||||
|
||||
bool is_server_;
|
||||
sockaddr_storage server_address_;
|
||||
socklen_t sa_len_;
|
||||
sockaddr_storage local_address_;
|
||||
socklen_t la_len_;
|
||||
|
||||
int server_port_;
|
||||
int local_port_;
|
||||
AcceptCallback accept_cb_;
|
||||
|
||||
std::string cached_remote_host_;
|
||||
|
||||
PollSet* poll_set_;
|
||||
int flags_;
|
||||
|
||||
std::string connected_host_;
|
||||
int connected_port_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_TRANSPORT_TCP_H
|
||||
|
||||
177
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport/transport_udp.h
vendored
Normal file
177
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport/transport_udp.h
vendored
Normal file
@@ -0,0 +1,177 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TRANSPORT_UDP_H
|
||||
#define ROSCPP_TRANSPORT_UDP_H
|
||||
|
||||
#include <ros/types.h>
|
||||
#include <ros/transport/transport.h>
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include "ros/io.h"
|
||||
#include <ros/common.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class TransportUDP;
|
||||
typedef boost::shared_ptr<TransportUDP> TransportUDPPtr;
|
||||
|
||||
class PollSet;
|
||||
|
||||
#define ROS_UDP_DATA0 0
|
||||
#define ROS_UDP_DATAN 1
|
||||
#define ROS_UDP_PING 2
|
||||
#define ROS_UDP_ERR 3
|
||||
typedef struct TransportUDPHeader {
|
||||
uint32_t connection_id_;
|
||||
uint8_t op_;
|
||||
uint8_t message_id_;
|
||||
uint16_t block_;
|
||||
} TransportUDPHeader;
|
||||
|
||||
/**
|
||||
* \brief UDPROS transport
|
||||
*/
|
||||
class ROSCPP_DECL TransportUDP : public Transport
|
||||
{
|
||||
public:
|
||||
enum Flags
|
||||
{
|
||||
SYNCHRONOUS = 1<<0,
|
||||
};
|
||||
|
||||
TransportUDP(PollSet* poll_set, int flags = 0, int max_datagram_size = 0);
|
||||
virtual ~TransportUDP();
|
||||
|
||||
/**
|
||||
* \brief Connect to a remote host.
|
||||
* \param host The hostname/IP to connect to
|
||||
* \param port The port to connect to
|
||||
* \return Whether or not the connection was successful
|
||||
*/
|
||||
bool connect(const std::string& host, int port, int conn_id);
|
||||
|
||||
/**
|
||||
* \brief Returns the URI of the remote host
|
||||
*/
|
||||
std::string getClientURI();
|
||||
|
||||
/**
|
||||
* \brief Start a server socket and listen on a port
|
||||
* \param port The port to listen on
|
||||
*/
|
||||
bool createIncoming(int port, bool is_server);
|
||||
/**
|
||||
* \brief Create a connection to a server socket.
|
||||
*/
|
||||
TransportUDPPtr createOutgoing(std::string host, int port, int conn_id, int max_datagram_size);
|
||||
/**
|
||||
* \brief Returns the port this transport is listening on
|
||||
*/
|
||||
int getServerPort() const {return server_port_;}
|
||||
|
||||
// overrides from Transport
|
||||
virtual int32_t read(uint8_t* buffer, uint32_t size);
|
||||
virtual int32_t write(uint8_t* buffer, uint32_t size);
|
||||
|
||||
virtual void enableWrite();
|
||||
virtual void disableWrite();
|
||||
virtual void enableRead();
|
||||
virtual void disableRead();
|
||||
|
||||
virtual void close();
|
||||
|
||||
virtual std::string getTransportInfo();
|
||||
|
||||
virtual bool requiresHeader() {return false;}
|
||||
|
||||
virtual const char* getType() {return "UDPROS";}
|
||||
|
||||
int getMaxDatagramSize() const {return max_datagram_size_;}
|
||||
|
||||
private:
|
||||
/**
|
||||
* \brief Initializes the assigned socket -- sets it to non-blocking and enables reading
|
||||
*/
|
||||
bool initializeSocket();
|
||||
|
||||
/**
|
||||
* \brief Set the socket to be used by this transport
|
||||
* \param sock A valid UDP socket
|
||||
* \return Whether setting the socket was successful
|
||||
*/
|
||||
bool setSocket(int sock);
|
||||
|
||||
void socketUpdate(int events);
|
||||
|
||||
socket_fd_t sock_;
|
||||
bool closed_;
|
||||
boost::mutex close_mutex_;
|
||||
|
||||
bool expecting_read_;
|
||||
bool expecting_write_;
|
||||
|
||||
bool is_server_;
|
||||
sockaddr_in server_address_;
|
||||
sockaddr_in local_address_;
|
||||
int server_port_;
|
||||
int local_port_;
|
||||
|
||||
std::string cached_remote_host_;
|
||||
|
||||
PollSet* poll_set_;
|
||||
int flags_;
|
||||
|
||||
uint32_t connection_id_;
|
||||
uint8_t current_message_id_;
|
||||
uint16_t total_blocks_;
|
||||
uint16_t last_block_;
|
||||
|
||||
uint32_t max_datagram_size_;
|
||||
|
||||
uint8_t* data_buffer_;
|
||||
uint8_t* data_start_;
|
||||
uint32_t data_filled_;
|
||||
|
||||
uint8_t* reorder_buffer_;
|
||||
uint8_t* reorder_start_;
|
||||
TransportUDPHeader reorder_header_;
|
||||
uint32_t reorder_bytes_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // ROSCPP_TRANSPORT_UDP_H
|
||||
|
||||
169
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport_hints.h
vendored
Normal file
169
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport_hints.h
vendored
Normal file
@@ -0,0 +1,169 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TRANSPORT_HINTS_H
|
||||
#define ROSCPP_TRANSPORT_HINTS_H
|
||||
|
||||
#include "common.h"
|
||||
#include "ros/forwards.h"
|
||||
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Provides a way of specifying network transport hints to ros::NodeHandle::subscribe() and
|
||||
* someday ros::NodeHandle::advertise()
|
||||
*
|
||||
* Uses the named parameter idiom, allowing you to do things like:
|
||||
\verbatim
|
||||
ros::TransportHints()
|
||||
.unreliable()
|
||||
.maxDatagramSize(1000)
|
||||
.tcpNoDelay();
|
||||
\endverbatim
|
||||
*
|
||||
* Hints for the transport type are used in the order they are specified, i.e. TransportHints().unreliable().reliable()
|
||||
* specifies that you would prefer an unreliable transport, followed by a reliable one.
|
||||
*/
|
||||
class ROSCPP_DECL TransportHints
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* \brief Specifies a reliable transport. Currently this means TCP
|
||||
*/
|
||||
TransportHints& reliable()
|
||||
{
|
||||
tcp();
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Explicitly specifies the TCP transport
|
||||
*/
|
||||
TransportHints& tcp()
|
||||
{
|
||||
transports_.push_back("TCP");
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief If a TCP transport is used, specifies whether or not to use TCP_NODELAY to provide
|
||||
* a potentially lower-latency connection.
|
||||
*
|
||||
* \param nodelay [optional] Whether or not to use TCP_NODELAY. Defaults to true.
|
||||
*/
|
||||
TransportHints& tcpNoDelay(bool nodelay = true)
|
||||
{
|
||||
options_["tcp_nodelay"] = nodelay ? "true" : "false";
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not this TransportHints has specified TCP_NODELAY
|
||||
*/
|
||||
bool getTCPNoDelay()
|
||||
{
|
||||
M_string::iterator it = options_.find("tcp_nodelay");
|
||||
if (it == options_.end())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const std::string& val = it->second;
|
||||
if (val == "true")
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief If a UDP transport is used, specifies the maximum datagram size.
|
||||
*
|
||||
* \param size The size, in bytes
|
||||
*/
|
||||
TransportHints& maxDatagramSize(int size)
|
||||
{
|
||||
options_["max_datagram_size"] = boost::lexical_cast<std::string>(size);
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Returns the maximum datagram size specified on this TransportHints, or 0 if
|
||||
* no size was specified.
|
||||
*/
|
||||
int getMaxDatagramSize()
|
||||
{
|
||||
M_string::iterator it = options_.find("max_datagram_size");
|
||||
if (it == options_.end())
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
return boost::lexical_cast<int>(it->second);
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Specifies an unreliable transport. Currently this means UDP.
|
||||
*/
|
||||
TransportHints& unreliable()
|
||||
{
|
||||
udp();
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Explicitly specifies a UDP transport.
|
||||
*/
|
||||
TransportHints& udp()
|
||||
{
|
||||
transports_.push_back("UDP");
|
||||
return *this;
|
||||
}
|
||||
|
||||
/**
|
||||
* \brief Returns a vector of transports, ordered by preference
|
||||
*/
|
||||
const V_string& getTransports() { return transports_; }
|
||||
/**
|
||||
* \brief Returns the map of options created by other methods inside TransportHints
|
||||
*/
|
||||
const M_string& getOptions() { return options_; }
|
||||
|
||||
private:
|
||||
V_string transports_;
|
||||
M_string options_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
96
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport_publisher_link.h
vendored
Normal file
96
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport_publisher_link.h
vendored
Normal file
@@ -0,0 +1,96 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TRANSPORT_PUBLISHER_LINK_H
|
||||
#define ROSCPP_TRANSPORT_PUBLISHER_LINK_H
|
||||
|
||||
#include "common.h"
|
||||
#include "publisher_link.h"
|
||||
#include "connection.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
class Header;
|
||||
class Message;
|
||||
class Subscription;
|
||||
typedef boost::shared_ptr<Subscription> SubscriptionPtr;
|
||||
typedef boost::weak_ptr<Subscription> SubscriptionWPtr;
|
||||
class Connection;
|
||||
typedef boost::shared_ptr<Connection> ConnectionPtr;
|
||||
|
||||
struct SteadyTimerEvent;
|
||||
|
||||
/**
|
||||
* \brief Handles a connection to a single publisher on a given topic. Receives messages from a publisher
|
||||
* and hands them off to its parent Subscription
|
||||
*/
|
||||
class ROSCPP_DECL TransportPublisherLink : public PublisherLink
|
||||
{
|
||||
public:
|
||||
TransportPublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri, const TransportHints& transport_hints);
|
||||
virtual ~TransportPublisherLink();
|
||||
|
||||
//
|
||||
bool initialize(const ConnectionPtr& connection);
|
||||
|
||||
const ConnectionPtr& getConnection() { return connection_; }
|
||||
|
||||
virtual std::string getTransportType();
|
||||
virtual std::string getTransportInfo();
|
||||
virtual void drop();
|
||||
|
||||
private:
|
||||
void onConnectionDropped(const ConnectionPtr& conn, Connection::DropReason reason);
|
||||
bool onHeaderReceived(const ConnectionPtr& conn, const Header& header);
|
||||
|
||||
/**
|
||||
* \brief Handles handing off a received message to the subscription, where it will be deserialized and called back
|
||||
*/
|
||||
virtual void handleMessage(const SerializedMessage& m, bool ser, bool nocopy);
|
||||
|
||||
void onHeaderWritten(const ConnectionPtr& conn);
|
||||
void onMessageLength(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
void onMessage(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success);
|
||||
|
||||
void onRetryTimer(const ros::SteadyTimerEvent&);
|
||||
|
||||
ConnectionPtr connection_;
|
||||
|
||||
int32_t retry_timer_handle_;
|
||||
bool needs_retry_;
|
||||
WallDuration retry_period_;
|
||||
SteadyTime next_retry_;
|
||||
bool dropping_;
|
||||
};
|
||||
typedef boost::shared_ptr<TransportPublisherLink> TransportPublisherLinkPtr;
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_TRANSPORT_PUBLISHER_LINK_H
|
||||
|
||||
|
||||
|
||||
79
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport_subscriber_link.h
vendored
Normal file
79
thirdparty/ros/ros_comm/clients/roscpp/include/ros/transport_subscriber_link.h
vendored
Normal file
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_TRANSPORT_SUBSCRIBER_LINK_H
|
||||
#define ROSCPP_TRANSPORT_SUBSCRIBER_LINK_H
|
||||
#include "common.h"
|
||||
#include "subscriber_link.h"
|
||||
|
||||
#include <boost/signals2/connection.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief SubscriberLink handles broadcasting messages to a single subscriber on a single topic
|
||||
*/
|
||||
class ROSCPP_DECL TransportSubscriberLink : public SubscriberLink
|
||||
{
|
||||
public:
|
||||
TransportSubscriberLink();
|
||||
virtual ~TransportSubscriberLink();
|
||||
|
||||
//
|
||||
bool initialize(const ConnectionPtr& connection);
|
||||
bool handleHeader(const Header& header);
|
||||
|
||||
const ConnectionPtr& getConnection() { return connection_; }
|
||||
|
||||
virtual void enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy);
|
||||
virtual void drop();
|
||||
virtual std::string getTransportType();
|
||||
virtual std::string getTransportInfo();
|
||||
|
||||
private:
|
||||
void onConnectionDropped(const ConnectionPtr& conn);
|
||||
|
||||
void onHeaderWritten(const ConnectionPtr& conn);
|
||||
void onMessageWritten(const ConnectionPtr& conn);
|
||||
void startMessageWrite(bool immediate_write);
|
||||
|
||||
bool writing_message_;
|
||||
bool header_written_;
|
||||
|
||||
ConnectionPtr connection_;
|
||||
boost::signals2::connection dropped_conn_;
|
||||
|
||||
std::queue<SerializedMessage> outbox_;
|
||||
boost::mutex outbox_mutex_;
|
||||
bool queue_full_;
|
||||
};
|
||||
typedef boost::shared_ptr<TransportSubscriberLink> TransportSubscriberLinkPtr;
|
||||
|
||||
} // namespace ros
|
||||
|
||||
#endif // ROSCPP_TRANSPORT_SUBSCRIBER_LINK_H
|
||||
128
thirdparty/ros/ros_comm/clients/roscpp/include/ros/wall_timer.h
vendored
Normal file
128
thirdparty/ros/ros_comm/clients/roscpp/include/ros/wall_timer.h
vendored
Normal file
@@ -0,0 +1,128 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_WALL_TIMER_H
|
||||
#define ROSCPP_WALL_TIMER_H
|
||||
|
||||
#include "common.h"
|
||||
#include "forwards.h"
|
||||
#include "wall_timer_options.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Manages a wall-clock timer callback
|
||||
*
|
||||
* A WallTimer should always be created through a call to NodeHandle::createWallTimer(), or copied from one
|
||||
* that was. Once all copies of a specific
|
||||
* WallTimer go out of scope, the callback associated with that handle will stop
|
||||
* being called.
|
||||
*/
|
||||
class ROSCPP_DECL WallTimer
|
||||
{
|
||||
public:
|
||||
WallTimer() {}
|
||||
WallTimer(const WallTimer& rhs);
|
||||
~WallTimer();
|
||||
|
||||
/**
|
||||
* \brief Start the timer. Does nothing if the timer is already started.
|
||||
*/
|
||||
void start();
|
||||
/**
|
||||
* \brief Stop the timer. Once this call returns, no more callbacks will be called. Does
|
||||
* nothing if the timer is already stopped.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* \brief Returns whether or not the timer has any pending events to call.
|
||||
*/
|
||||
bool hasPending();
|
||||
|
||||
/**
|
||||
* \brief Set the period of this timer
|
||||
* \param reset Whether to reset the timer. If true, timer ignores elapsed time and next cb occurs at now()+period
|
||||
*/
|
||||
void setPeriod(const WallDuration& period, bool reset=true);
|
||||
|
||||
bool isValid() { return impl_ && impl_->isValid(); }
|
||||
operator void*() { return isValid() ? (void*)1 : (void*)0; }
|
||||
|
||||
bool operator<(const WallTimer& rhs)
|
||||
{
|
||||
return impl_ < rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator==(const WallTimer& rhs)
|
||||
{
|
||||
return impl_ == rhs.impl_;
|
||||
}
|
||||
|
||||
bool operator!=(const WallTimer& rhs)
|
||||
{
|
||||
return impl_ != rhs.impl_;
|
||||
}
|
||||
|
||||
private:
|
||||
WallTimer(const WallTimerOptions& ops);
|
||||
|
||||
class Impl
|
||||
{
|
||||
public:
|
||||
Impl();
|
||||
~Impl();
|
||||
|
||||
bool isValid();
|
||||
bool hasPending();
|
||||
void setPeriod(const WallDuration& period, bool reset=true);
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
|
||||
bool started_;
|
||||
int32_t timer_handle_;
|
||||
|
||||
WallDuration period_;
|
||||
WallTimerCallback callback_;
|
||||
CallbackQueueInterface* callback_queue_;
|
||||
VoidConstWPtr tracked_object_;
|
||||
bool has_tracked_object_;
|
||||
bool oneshot_;
|
||||
};
|
||||
typedef boost::shared_ptr<Impl> ImplPtr;
|
||||
typedef boost::weak_ptr<Impl> ImplWPtr;
|
||||
|
||||
ImplPtr impl_;
|
||||
|
||||
friend class NodeHandle;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
86
thirdparty/ros/ros_comm/clients/roscpp/include/ros/wall_timer_options.h
vendored
Normal file
86
thirdparty/ros/ros_comm/clients/roscpp/include/ros/wall_timer_options.h
vendored
Normal file
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_WALL_TIMER_OPTIONS_H
|
||||
#define ROSCPP_WALL_TIMER_OPTIONS_H
|
||||
|
||||
#include "common.h"
|
||||
#include "ros/forwards.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief Encapsulates all options available for starting a timer
|
||||
*/
|
||||
struct ROSCPP_DECL WallTimerOptions
|
||||
{
|
||||
WallTimerOptions()
|
||||
: period(0.1)
|
||||
, callback_queue(0)
|
||||
, oneshot(false)
|
||||
, autostart(true)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
* \brief Constructor
|
||||
* \param
|
||||
*/
|
||||
WallTimerOptions(WallDuration _period, const WallTimerCallback& _callback, CallbackQueueInterface* _queue,
|
||||
bool oneshot = false, bool autostart = true)
|
||||
: period(_period)
|
||||
, callback(_callback)
|
||||
, callback_queue(_queue)
|
||||
, oneshot(oneshot)
|
||||
, autostart(autostart)
|
||||
{}
|
||||
|
||||
WallDuration period; ///< The period to call the callback at
|
||||
WallTimerCallback callback; ///< The callback to call
|
||||
|
||||
CallbackQueueInterface* callback_queue; ///< Queue to add callbacks to. If NULL, the global callback queue will be used
|
||||
|
||||
/**
|
||||
* A shared pointer to an object to track for these callbacks. If set, the a weak_ptr will be created to this object,
|
||||
* and if the reference count goes to 0 the subscriber callbacks will not get called.
|
||||
*
|
||||
* \note Note that setting this will cause a new reference to be added to the object before the
|
||||
* callback, and for it to go out of scope (and potentially be deleted) in the code path (and therefore
|
||||
* thread) that the callback is invoked from.
|
||||
*/
|
||||
VoidConstPtr tracked_object;
|
||||
|
||||
bool oneshot;
|
||||
bool autostart;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
178
thirdparty/ros/ros_comm/clients/roscpp/include/ros/xmlrpc_manager.h
vendored
Normal file
178
thirdparty/ros/ros_comm/clients/roscpp/include/ros/xmlrpc_manager.h
vendored
Normal file
@@ -0,0 +1,178 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#ifndef ROSCPP_XMLRPC_MANAGER_H
|
||||
#define ROSCPP_XMLRPC_MANAGER_H
|
||||
|
||||
#include <string>
|
||||
#include <set>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/thread.hpp>
|
||||
#include <boost/enable_shared_from_this.hpp>
|
||||
|
||||
#include "common.h"
|
||||
#include "xmlrpcpp/XmlRpc.h"
|
||||
|
||||
#include <ros/time.h>
|
||||
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
/**
|
||||
* \brief internal
|
||||
*/
|
||||
namespace xmlrpc
|
||||
{
|
||||
XmlRpc::XmlRpcValue responseStr(int code, const std::string& msg, const std::string& response);
|
||||
XmlRpc::XmlRpcValue responseInt(int code, const std::string& msg, int response);
|
||||
XmlRpc::XmlRpcValue responseBool(int code, const std::string& msg, bool response);
|
||||
}
|
||||
|
||||
class XMLRPCCallWrapper;
|
||||
typedef boost::shared_ptr<XMLRPCCallWrapper> XMLRPCCallWrapperPtr;
|
||||
|
||||
class ROSCPP_DECL ASyncXMLRPCConnection : public boost::enable_shared_from_this<ASyncXMLRPCConnection>
|
||||
{
|
||||
public:
|
||||
virtual ~ASyncXMLRPCConnection() {}
|
||||
|
||||
virtual void addToDispatch(XmlRpc::XmlRpcDispatch* disp) = 0;
|
||||
virtual void removeFromDispatch(XmlRpc::XmlRpcDispatch* disp) = 0;
|
||||
|
||||
virtual bool check() = 0;
|
||||
};
|
||||
typedef boost::shared_ptr<ASyncXMLRPCConnection> ASyncXMLRPCConnectionPtr;
|
||||
typedef std::set<ASyncXMLRPCConnectionPtr> S_ASyncXMLRPCConnection;
|
||||
|
||||
class ROSCPP_DECL CachedXmlRpcClient
|
||||
{
|
||||
public:
|
||||
CachedXmlRpcClient(XmlRpc::XmlRpcClient *c)
|
||||
: in_use_(false)
|
||||
, client_(c)
|
||||
{
|
||||
}
|
||||
|
||||
bool in_use_;
|
||||
ros::SteadyTime last_use_time_; // for reaping
|
||||
XmlRpc::XmlRpcClient* client_;
|
||||
|
||||
static const ros::WallDuration s_zombie_time_; // how long before it is toasted
|
||||
};
|
||||
|
||||
class XMLRPCManager;
|
||||
typedef boost::shared_ptr<XMLRPCManager> XMLRPCManagerPtr;
|
||||
|
||||
typedef boost::function<void(XmlRpc::XmlRpcValue&, XmlRpc::XmlRpcValue&)> XMLRPCFunc;
|
||||
|
||||
class ROSCPP_DECL XMLRPCManager
|
||||
{
|
||||
public:
|
||||
static const XMLRPCManagerPtr& instance();
|
||||
|
||||
XMLRPCManager();
|
||||
~XMLRPCManager();
|
||||
|
||||
/** @brief Validate an XML/RPC response
|
||||
*
|
||||
* @param method The RPC method that was invoked.
|
||||
* @param response The resonse that was received.
|
||||
* @param payload The payload that was received.
|
||||
*
|
||||
* @return true if validation succeeds, false otherwise.
|
||||
*
|
||||
* @todo Consider making this private.
|
||||
*/
|
||||
bool validateXmlrpcResponse(const std::string& method,
|
||||
XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload);
|
||||
|
||||
/**
|
||||
* @brief Get the xmlrpc server URI of this node
|
||||
*/
|
||||
inline const std::string& getServerURI() const { return uri_; }
|
||||
inline uint32_t getServerPort() const { return port_; }
|
||||
|
||||
XmlRpc::XmlRpcClient* getXMLRPCClient(const std::string& host, const int port, const std::string& uri);
|
||||
void releaseXMLRPCClient(XmlRpc::XmlRpcClient* c);
|
||||
|
||||
void addASyncConnection(const ASyncXMLRPCConnectionPtr& conn);
|
||||
void removeASyncConnection(const ASyncXMLRPCConnectionPtr& conn);
|
||||
|
||||
bool bind(const std::string& function_name, const XMLRPCFunc& cb);
|
||||
void unbind(const std::string& function_name);
|
||||
|
||||
void start();
|
||||
void shutdown();
|
||||
|
||||
bool isShuttingDown() { return shutting_down_; }
|
||||
|
||||
private:
|
||||
void serverThreadFunc();
|
||||
|
||||
std::string uri_;
|
||||
int port_;
|
||||
boost::thread server_thread_;
|
||||
|
||||
#if defined(__APPLE__)
|
||||
// OSX has problems with lots of concurrent xmlrpc calls
|
||||
boost::mutex xmlrpc_call_mutex_;
|
||||
#endif
|
||||
XmlRpc::XmlRpcServer server_;
|
||||
typedef std::vector<CachedXmlRpcClient> V_CachedXmlRpcClient;
|
||||
V_CachedXmlRpcClient clients_;
|
||||
boost::mutex clients_mutex_;
|
||||
|
||||
bool shutting_down_;
|
||||
|
||||
ros::WallDuration master_retry_timeout_;
|
||||
|
||||
S_ASyncXMLRPCConnection added_connections_;
|
||||
boost::mutex added_connections_mutex_;
|
||||
S_ASyncXMLRPCConnection removed_connections_;
|
||||
boost::mutex removed_connections_mutex_;
|
||||
|
||||
S_ASyncXMLRPCConnection connections_;
|
||||
|
||||
|
||||
struct FunctionInfo
|
||||
{
|
||||
std::string name;
|
||||
XMLRPCFunc function;
|
||||
XMLRPCCallWrapperPtr wrapper;
|
||||
};
|
||||
typedef std::map<std::string, FunctionInfo> M_StringToFuncInfo;
|
||||
boost::mutex functions_mutex_;
|
||||
M_StringToFuncInfo functions_;
|
||||
|
||||
volatile bool unbind_requested_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
2
thirdparty/ros/ros_comm/clients/roscpp/msg/Logger.msg
vendored
Normal file
2
thirdparty/ros/ros_comm/clients/roscpp/msg/Logger.msg
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
string name
|
||||
string level
|
||||
47
thirdparty/ros/ros_comm/clients/roscpp/package.xml
vendored
Normal file
47
thirdparty/ros/ros_comm/clients/roscpp/package.xml
vendored
Normal file
@@ -0,0 +1,47 @@
|
||||
<package>
|
||||
<name>roscpp</name>
|
||||
<version>1.12.14</version>
|
||||
<description>
|
||||
roscpp is a C++ implementation of ROS. It provides
|
||||
a <a href="http://www.ros.org/wiki/Client%20Libraries">client
|
||||
library</a> that enables C++ 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>.
|
||||
|
||||
roscpp is the most widely used ROS client library and is designed to
|
||||
be the high-performance library for ROS.
|
||||
</description>
|
||||
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
|
||||
<license>BSD</license>
|
||||
|
||||
<url>http://ros.org/wiki/roscpp</url>
|
||||
<author>Morgan Quigley</author>
|
||||
<author>Josh Faust</author>
|
||||
<author>Brian Gerkey</author>
|
||||
<author>Troy Straszheim</author>
|
||||
|
||||
<buildtool_depend version_gte="0.5.78">catkin</buildtool_depend>
|
||||
|
||||
<build_depend version_gte="0.3.17">cpp_common</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>pkg-config</build_depend>
|
||||
<build_depend>rosconsole</build_depend>
|
||||
<build_depend>roscpp_serialization</build_depend>
|
||||
<build_depend version_gte="0.3.17">roscpp_traits</build_depend>
|
||||
<build_depend version_gte="1.10.3">rosgraph_msgs</build_depend>
|
||||
<build_depend>roslang</build_depend>
|
||||
<build_depend version_gte="0.6.4">rostime</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>xmlrpcpp</build_depend>
|
||||
|
||||
<run_depend version_gte="0.3.17">cpp_common</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
<run_depend>rosconsole</run_depend>
|
||||
<run_depend>roscpp_serialization</run_depend>
|
||||
<run_depend version_gte="0.3.17">roscpp_traits</run_depend>
|
||||
<run_depend version_gte="1.10.3">rosgraph_msgs</run_depend>
|
||||
<run_depend version_gte="0.6.4">rostime</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>xmlrpcpp</run_depend>
|
||||
</package>
|
||||
87
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/roscpp.cmake
vendored
Normal file
87
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/roscpp.cmake
vendored
Normal file
@@ -0,0 +1,87 @@
|
||||
rosbuild_find_ros_package(genmsg_cpp)
|
||||
rosbuild_find_ros_package(roscpp)
|
||||
|
||||
# Message-generation support.
|
||||
macro(genmsg_cpp)
|
||||
rosbuild_get_msgs(_msglist)
|
||||
set(_autogen "")
|
||||
foreach(_msg ${_msglist})
|
||||
# Construct the path to the .msg file
|
||||
set(_input ${PROJECT_SOURCE_DIR}/msg/${_msg})
|
||||
|
||||
rosbuild_gendeps(${PROJECT_NAME} ${_msg})
|
||||
|
||||
set(genmsg_cpp_exe ${roscpp_PACKAGE_PATH}/rosbuild/scripts/genmsg_cpp.py)
|
||||
|
||||
set(_output_cpp ${PROJECT_SOURCE_DIR}/msg_gen/cpp/include/${PROJECT_NAME}/${_msg})
|
||||
string(REPLACE ".msg" ".h" _output_cpp ${_output_cpp})
|
||||
|
||||
# Add the rule to build the .h the .msg
|
||||
add_custom_command(OUTPUT ${_output_cpp}
|
||||
COMMAND ${genmsg_cpp_exe} ${_input}
|
||||
DEPENDS ${_input} ${genmsg_cpp_exe} ${gendeps_exe} ${${PROJECT_NAME}_${_msg}_GENDEPS} ${ROS_MANIFEST_LIST})
|
||||
list(APPEND _autogen ${_output_cpp})
|
||||
endforeach(_msg)
|
||||
# Create a target that depends on the union of all the autogenerated
|
||||
# files
|
||||
add_custom_target(ROSBUILD_genmsg_cpp DEPENDS ${_autogen})
|
||||
# Make our target depend on rosbuild_premsgsrvgen, to allow any
|
||||
# pre-msg/srv generation steps to be done first.
|
||||
add_dependencies(ROSBUILD_genmsg_cpp rosbuild_premsgsrvgen)
|
||||
# Add our target to the top-level rospack_genmsg target, which will be
|
||||
# fired if the user calls genmsg()
|
||||
add_dependencies(rospack_genmsg ROSBUILD_genmsg_cpp)
|
||||
|
||||
if(_autogen)
|
||||
# Also set up to clean the msg_gen directory
|
||||
get_directory_property(_old_clean_files ADDITIONAL_MAKE_CLEAN_FILES)
|
||||
list(APPEND _old_clean_files ${PROJECT_SOURCE_DIR}/msg_gen)
|
||||
set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${_old_clean_files}")
|
||||
endif(_autogen)
|
||||
endmacro(genmsg_cpp)
|
||||
|
||||
# Call the macro we just defined.
|
||||
genmsg_cpp()
|
||||
|
||||
# Service-generation support.
|
||||
macro(gensrv_cpp)
|
||||
rosbuild_get_srvs(_srvlist)
|
||||
set(_autogen "")
|
||||
foreach(_srv ${_srvlist})
|
||||
# Construct the path to the .srv file
|
||||
set(_input ${PROJECT_SOURCE_DIR}/srv/${_srv})
|
||||
|
||||
rosbuild_gendeps(${PROJECT_NAME} ${_srv})
|
||||
|
||||
set(gensrv_cpp_exe ${roscpp_PACKAGE_PATH}/rosbuild/scripts/gensrv_cpp.py)
|
||||
set(genmsg_cpp_exe ${roscpp_PACKAGE_PATH}/rosbuild/scripts/genmsg_cpp.py)
|
||||
|
||||
set(_output_cpp ${PROJECT_SOURCE_DIR}/srv_gen/cpp/include/${PROJECT_NAME}/${_srv})
|
||||
string(REPLACE ".srv" ".h" _output_cpp ${_output_cpp})
|
||||
|
||||
# Add the rule to build the .h from the .srv
|
||||
add_custom_command(OUTPUT ${_output_cpp}
|
||||
COMMAND ${gensrv_cpp_exe} ${_input}
|
||||
DEPENDS ${_input} ${gensrv_cpp_exe} ${genmsg_cpp_exe} ${gendeps_exe} ${${PROJECT_NAME}_${_srv}_GENDEPS} ${ROS_MANIFEST_LIST})
|
||||
list(APPEND _autogen ${_output_cpp})
|
||||
endforeach(_srv)
|
||||
# Create a target that depends on the union of all the autogenerated
|
||||
# files
|
||||
add_custom_target(ROSBUILD_gensrv_cpp DEPENDS ${_autogen})
|
||||
# Make our target depend on rosbuild_premsgsrvgen, to allow any
|
||||
# pre-msg/srv generation steps to be done first.
|
||||
add_dependencies(ROSBUILD_gensrv_cpp 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_cpp)
|
||||
|
||||
if(_autogen)
|
||||
# Also set up to clean the srv_gen directory
|
||||
get_directory_property(_old_clean_files ADDITIONAL_MAKE_CLEAN_FILES)
|
||||
list(APPEND _old_clean_files ${PROJECT_SOURCE_DIR}/srv_gen)
|
||||
set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${_old_clean_files}")
|
||||
endif(_autogen)
|
||||
endmacro(gensrv_cpp)
|
||||
|
||||
# Call the macro we just defined.
|
||||
gensrv_cpp()
|
||||
56
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/scripts/genmsg_cpp.py
vendored
Executable file
56
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/scripts/genmsg_cpp.py
vendored
Executable file
@@ -0,0 +1,56 @@
|
||||
#!/usr/bin/env python
|
||||
# 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.
|
||||
#
|
||||
|
||||
## ROS message source code generation for C++
|
||||
##
|
||||
## Converts ROS .msg files in a package into C++ source code implementations.
|
||||
|
||||
import msg_gen
|
||||
|
||||
import sys
|
||||
|
||||
if __name__ == "__main__":
|
||||
msg_gen.generate_messages(sys.argv)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
221
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/scripts/gensrv_cpp.py
vendored
Executable file
221
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/scripts/gensrv_cpp.py
vendored
Executable file
@@ -0,0 +1,221 @@
|
||||
#!/usr/bin/env python
|
||||
# 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.
|
||||
#
|
||||
|
||||
## ROS message source code generation for C++
|
||||
##
|
||||
## Converts ROS .msg files in a package into C++ source code implementations.
|
||||
|
||||
import msg_gen as genmsg_cpp
|
||||
|
||||
import sys
|
||||
import os
|
||||
import traceback
|
||||
|
||||
# roslib.msgs contains the utilities for parsing .msg specifications. It is meant to have no rospy-specific knowledge
|
||||
import roslib.srvs
|
||||
import roslib.packages
|
||||
import roslib.gentools
|
||||
from rospkg import RosPack
|
||||
|
||||
try:
|
||||
from cStringIO import StringIO #Python 2.x
|
||||
except ImportError:
|
||||
from io import StringIO #Python 3.x
|
||||
|
||||
def write_begin(s, spec, file):
|
||||
"""
|
||||
Writes the beginning of the header file: a comment saying it's auto-generated and the include guards
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The spec
|
||||
@type spec: roslib.srvs.SrvSpec
|
||||
@param file: The file this service is being generated for
|
||||
@type file: str
|
||||
"""
|
||||
s.write("/* Auto-generated by genmsg_cpp for file %s */\n"%(file))
|
||||
s.write('#ifndef %s_SERVICE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
|
||||
s.write('#define %s_SERVICE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
|
||||
|
||||
def write_end(s, spec):
|
||||
"""
|
||||
Writes the end of the header file: the ending of the include guards
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The spec
|
||||
@type spec: roslib.srvs.SrvSpec
|
||||
"""
|
||||
s.write('#endif // %s_SERVICE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
|
||||
|
||||
def write_generic_includes(s):
|
||||
"""
|
||||
Writes the includes that all service need
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
"""
|
||||
s.write('#include "ros/service_traits.h"\n\n')
|
||||
|
||||
def write_trait_char_class(s, class_name, cpp_msg, value):
|
||||
"""
|
||||
Writes a class trait for traits which have static value() members that return const char*
|
||||
|
||||
e.g. write_trait_char_class(s, "MD5Sum", "std_srvs::Empty", "hello") yields:
|
||||
template<>
|
||||
struct MD5Sum<std_srvs::Empty>
|
||||
{
|
||||
static const char* value() { return "hello"; }
|
||||
static const char* value(const std_srvs::Empty&) { return value(); }
|
||||
};
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param class_name: The name of the trait class
|
||||
@type class_name: str
|
||||
@param cpp_msg: The C++ message declaration, e.g. "std_srvs::Empty"
|
||||
@type cpp_msg: str
|
||||
@param value: The string value to return from the value() function
|
||||
@type value: str
|
||||
"""
|
||||
s.write('template<>\nstruct %s<%s> {\n'%(class_name, cpp_msg))
|
||||
s.write(' static const char* value() \n {\n return "%s";\n }\n\n'%(value))
|
||||
s.write(' static const char* value(const %s&) { return value(); } \n'%(cpp_msg))
|
||||
s.write('};\n\n')
|
||||
|
||||
def write_traits(s, spec, cpp_name_prefix, rospack=None):
|
||||
"""
|
||||
Write all the service traits for a message
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The service spec
|
||||
@type spec: roslib.srvs.SrvSpec
|
||||
@param cpp_name_prefix: The C++ prefix to prepend when referencing the service, e.g. "std_srvs::"
|
||||
@type cpp_name_prefix: str
|
||||
"""
|
||||
gendeps_dict = roslib.gentools.get_dependencies(spec, spec.package, rospack=rospack)
|
||||
md5sum = roslib.gentools.compute_md5(gendeps_dict, rospack=rospack)
|
||||
|
||||
s.write('namespace ros\n{\n')
|
||||
s.write('namespace service_traits\n{\n')
|
||||
|
||||
request_with_allocator = '%s%s_<ContainerAllocator> '%(cpp_name_prefix, spec.request.short_name)
|
||||
response_with_allocator = '%s%s_<ContainerAllocator> '%(cpp_name_prefix, spec.response.short_name)
|
||||
|
||||
write_trait_char_class(s, 'MD5Sum', '%s%s'%(cpp_name_prefix, spec.short_name), md5sum);
|
||||
write_trait_char_class(s, 'DataType', '%s%s'%(cpp_name_prefix, spec.short_name), spec.full_name);
|
||||
genmsg_cpp.write_trait_char_class(s, 'MD5Sum', request_with_allocator, md5sum)
|
||||
genmsg_cpp.write_trait_char_class(s, 'DataType', request_with_allocator, spec.full_name)
|
||||
genmsg_cpp.write_trait_char_class(s, 'MD5Sum', response_with_allocator, md5sum)
|
||||
genmsg_cpp.write_trait_char_class(s, 'DataType', response_with_allocator, spec.full_name)
|
||||
s.write('} // namespace service_traits\n')
|
||||
s.write('} // namespace ros\n\n')
|
||||
|
||||
def generate(srv_path):
|
||||
"""
|
||||
Generate a service
|
||||
|
||||
@param srv_path: the path to the .srv file
|
||||
@type srv_path: str
|
||||
"""
|
||||
(package_dir, package) = roslib.packages.get_dir_pkg(srv_path)
|
||||
(_, spec) = roslib.srvs.load_from_file(srv_path, package)
|
||||
|
||||
s = StringIO()
|
||||
cpp_prefix = '%s::'%(package)
|
||||
write_begin(s, spec, srv_path)
|
||||
genmsg_cpp.write_generic_includes(s)
|
||||
write_generic_includes(s)
|
||||
genmsg_cpp.write_includes(s, spec.request)
|
||||
s.write('\n')
|
||||
genmsg_cpp.write_includes(s, spec.response)
|
||||
|
||||
rospack = RosPack()
|
||||
gendeps_dict = roslib.gentools.get_dependencies(spec, spec.package, rospack=rospack)
|
||||
md5sum = roslib.gentools.compute_md5(gendeps_dict, rospack=rospack)
|
||||
|
||||
s.write('namespace %s\n{\n'%(package))
|
||||
genmsg_cpp.write_struct(s, spec.request, cpp_prefix, {'ServerMD5Sum': md5sum})
|
||||
genmsg_cpp.write_constant_definitions(s, spec.request)
|
||||
s.write('\n')
|
||||
genmsg_cpp.write_struct(s, spec.response, cpp_prefix, {'ServerMD5Sum': md5sum})
|
||||
genmsg_cpp.write_constant_definitions(s, spec.response)
|
||||
s.write('struct %s\n{\n'%(spec.short_name))
|
||||
s.write('\n')
|
||||
s.write('typedef %s Request;\n'%(spec.request.short_name))
|
||||
s.write('typedef %s Response;\n'%(spec.response.short_name))
|
||||
s.write('Request request;\n')
|
||||
s.write('Response response;\n\n')
|
||||
s.write('typedef Request RequestType;\n')
|
||||
s.write('typedef Response ResponseType;\n')
|
||||
s.write('}; // struct %s\n'%(spec.short_name))
|
||||
s.write('} // namespace %s\n\n'%(package))
|
||||
|
||||
request_cpp_name = "Request"
|
||||
response_cpp_name = "Response"
|
||||
genmsg_cpp.write_traits(s, spec.request, cpp_prefix, rospack=rospack)
|
||||
s.write('\n')
|
||||
genmsg_cpp.write_traits(s, spec.response, cpp_prefix, rospack=rospack)
|
||||
genmsg_cpp.write_serialization(s, spec.request, cpp_prefix)
|
||||
s.write('\n')
|
||||
genmsg_cpp.write_serialization(s, spec.response, cpp_prefix)
|
||||
|
||||
write_traits(s, spec, cpp_prefix, rospack=rospack)
|
||||
|
||||
write_end(s, spec)
|
||||
|
||||
output_dir = '%s/srv_gen/cpp/include/%s'%(package_dir, package)
|
||||
if (not os.path.exists(output_dir)):
|
||||
# if we're being run concurrently, the above test can report false but os.makedirs can still fail if
|
||||
# another copy just created the directory
|
||||
try:
|
||||
os.makedirs(output_dir)
|
||||
except OSError as e:
|
||||
pass
|
||||
|
||||
f = open('%s/%s.h'%(output_dir, spec.short_name), 'w')
|
||||
f.write(s.getvalue() + "\n")
|
||||
|
||||
s.close()
|
||||
|
||||
def generate_services(argv):
|
||||
for arg in argv[1:]:
|
||||
generate(arg)
|
||||
|
||||
if __name__ == "__main__":
|
||||
roslib.msgs.set_verbose(False)
|
||||
generate_services(sys.argv)
|
||||
|
||||
|
||||
744
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/scripts/msg_gen.py
vendored
Executable file
744
thirdparty/ros/ros_comm/clients/roscpp/rosbuild/scripts/msg_gen.py
vendored
Executable file
@@ -0,0 +1,744 @@
|
||||
#!/usr/bin/env python
|
||||
# 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.
|
||||
#
|
||||
|
||||
## ROS message source code generation for C++
|
||||
##
|
||||
## Converts ROS .msg files in a package into C++ source code implementations.
|
||||
|
||||
import sys
|
||||
import os
|
||||
import traceback
|
||||
|
||||
import roslib.msgs
|
||||
import roslib.packages
|
||||
import roslib.gentools
|
||||
from rospkg import RosPack
|
||||
|
||||
try:
|
||||
from cStringIO import StringIO #Python 2.x
|
||||
except ImportError:
|
||||
from io import StringIO #Python 3.x
|
||||
|
||||
MSG_TYPE_TO_CPP = {'byte': 'int8_t', 'char': 'uint8_t',
|
||||
'bool': 'uint8_t',
|
||||
'uint8': 'uint8_t', 'int8': 'int8_t',
|
||||
'uint16': 'uint16_t', 'int16': 'int16_t',
|
||||
'uint32': 'uint32_t', 'int32': 'int32_t',
|
||||
'uint64': 'uint64_t', 'int64': 'int64_t',
|
||||
'float32': 'float',
|
||||
'float64': 'double',
|
||||
'string': 'std::basic_string<char, std::char_traits<char>, typename ContainerAllocator::template rebind<char>::other > ',
|
||||
'time': 'ros::Time',
|
||||
'duration': 'ros::Duration'}
|
||||
|
||||
def msg_type_to_cpp(type):
|
||||
"""
|
||||
Converts a message type (e.g. uint32, std_msgs/String, etc.) into the C++ declaration
|
||||
for that type (e.g. uint32_t, std_msgs::String_<ContainerAllocator>)
|
||||
|
||||
@param type: The message type
|
||||
@type type: str
|
||||
@return: The C++ declaration
|
||||
@rtype: str
|
||||
"""
|
||||
(base_type, is_array, array_len) = roslib.msgs.parse_type(type)
|
||||
cpp_type = None
|
||||
if (roslib.msgs.is_builtin(base_type)):
|
||||
cpp_type = MSG_TYPE_TO_CPP[base_type]
|
||||
elif (len(base_type.split('/')) == 1):
|
||||
if (roslib.msgs.is_header_type(base_type)):
|
||||
cpp_type = ' ::std_msgs::Header_<ContainerAllocator> '
|
||||
else:
|
||||
cpp_type = '%s_<ContainerAllocator> '%(base_type)
|
||||
else:
|
||||
pkg = base_type.split('/')[0]
|
||||
msg = base_type.split('/')[1]
|
||||
cpp_type = ' ::%s::%s_<ContainerAllocator> '%(pkg, msg)
|
||||
|
||||
if (is_array):
|
||||
if (array_len is None):
|
||||
return 'std::vector<%s, typename ContainerAllocator::template rebind<%s>::other > '%(cpp_type, cpp_type)
|
||||
else:
|
||||
return 'boost::array<%s, %s> '%(cpp_type, array_len)
|
||||
else:
|
||||
return cpp_type
|
||||
|
||||
def cpp_message_declarations(name_prefix, msg):
|
||||
"""
|
||||
Returns the different possible C++ declarations for a message given the message itself.
|
||||
|
||||
@param name_prefix: The C++ prefix to be prepended to the name, e.g. "std_msgs::"
|
||||
@type name_prefix: str
|
||||
@param msg: The message type
|
||||
@type msg: str
|
||||
@return: A tuple of 3 different names. cpp_message_decelarations("std_msgs::", "String") returns the tuple
|
||||
("std_msgs::String_", "std_msgs::String_<ContainerAllocator>", "std_msgs::String")
|
||||
@rtype: str
|
||||
"""
|
||||
pkg, basetype = roslib.names.package_resource_name(msg)
|
||||
cpp_name = ' ::%s%s'%(name_prefix, msg)
|
||||
if (pkg):
|
||||
cpp_name = ' ::%s::%s'%(pkg, basetype)
|
||||
return ('%s_'%(cpp_name), '%s_<ContainerAllocator> '%(cpp_name), '%s'%(cpp_name))
|
||||
|
||||
def write_begin(s, spec, file):
|
||||
"""
|
||||
Writes the beginning of the header file: a comment saying it's auto-generated and the include guards
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param file: The file this message is being generated for
|
||||
@type file: str
|
||||
"""
|
||||
s.write("/* Auto-generated by genmsg_cpp for file %s */\n"%(file))
|
||||
s.write('#ifndef %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
|
||||
s.write('#define %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
|
||||
|
||||
def write_end(s, spec):
|
||||
"""
|
||||
Writes the end of the header file: the ending of the include guards
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
"""
|
||||
s.write('#endif // %s_MESSAGE_%s_H\n'%(spec.package.upper(), spec.short_name.upper()))
|
||||
|
||||
def write_generic_includes(s):
|
||||
"""
|
||||
Writes the includes that all messages need
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
"""
|
||||
s.write('#include <string>\n')
|
||||
s.write('#include <vector>\n')
|
||||
s.write('#include <map>\n')
|
||||
s.write('#include <ostream>\n')
|
||||
s.write('#include "ros/serialization.h"\n')
|
||||
s.write('#include "ros/builtin_message_traits.h"\n')
|
||||
s.write('#include "ros/message_operations.h"\n')
|
||||
s.write('#include "ros/time.h"\n\n')
|
||||
s.write('#include "ros/macros.h"\n\n')
|
||||
s.write('#include "ros/assert.h"\n\n')
|
||||
|
||||
def write_includes(s, spec):
|
||||
"""
|
||||
Writes the message-specific includes
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec to iterate over
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
"""
|
||||
for field in spec.parsed_fields():
|
||||
if (not field.is_builtin):
|
||||
if (field.is_header):
|
||||
s.write('#include "std_msgs/Header.h"\n')
|
||||
else:
|
||||
(pkg, name) = roslib.names.package_resource_name(field.base_type)
|
||||
pkg = pkg or spec.package # convert '' to package
|
||||
s.write('#include "%s/%s.h"\n'%(pkg, name))
|
||||
|
||||
s.write('\n')
|
||||
|
||||
|
||||
def write_struct(s, spec, cpp_name_prefix, extra_deprecated_traits = {}):
|
||||
"""
|
||||
Writes the entire message struct: declaration, constructors, members, constants and (deprecated) member functions
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::"
|
||||
@type cpp_name_prefix: str
|
||||
"""
|
||||
|
||||
msg = spec.short_name
|
||||
s.write('template <class ContainerAllocator>\n')
|
||||
s.write('struct %s_ {\n'%(msg))
|
||||
s.write(' typedef %s_<ContainerAllocator> Type;\n\n'%(msg))
|
||||
|
||||
write_constructors(s, spec, cpp_name_prefix)
|
||||
write_members(s, spec)
|
||||
write_constant_declarations(s, spec)
|
||||
|
||||
#rospack = RosPack()
|
||||
#gendeps_dict = roslib.gentools.get_dependencies(spec, spec.package, compute_files=False, rospack=rospack)
|
||||
#md5sum = roslib.gentools.compute_md5(gendeps_dict, rospack=rospack)
|
||||
#full_text = compute_full_text_escaped(gendeps_dict)
|
||||
|
||||
# write_deprecated_member_functions(s, spec, dict(list({'MD5Sum': md5sum, 'DataType': '%s/%s'%(spec.package, spec.short_name), 'MessageDefinition': full_text}.items()) + list(extra_deprecated_traits.items())))
|
||||
|
||||
(cpp_msg_unqualified, cpp_msg_with_alloc, cpp_msg_base) = cpp_message_declarations(cpp_name_prefix, msg)
|
||||
s.write(' typedef boost::shared_ptr<%s> Ptr;\n'%(cpp_msg_with_alloc))
|
||||
s.write(' typedef boost::shared_ptr<%s const> ConstPtr;\n'%(cpp_msg_with_alloc))
|
||||
|
||||
s.write('}; // struct %s\n'%(msg))
|
||||
|
||||
s.write('typedef %s_<std::allocator<void> > %s;\n\n'%(cpp_msg_base, msg))
|
||||
s.write('typedef boost::shared_ptr<%s> %sPtr;\n'%(cpp_msg_base, msg))
|
||||
s.write('typedef boost::shared_ptr<%s const> %sConstPtr;\n\n'%(cpp_msg_base, msg))
|
||||
|
||||
def default_value(type):
|
||||
"""
|
||||
Returns the value to initialize a message member with. 0 for integer types, 0.0 for floating point, false for bool,
|
||||
empty string for everything else
|
||||
|
||||
@param type: The type
|
||||
@type type: str
|
||||
"""
|
||||
if type in ['byte', 'int8', 'int16', 'int32', 'int64',
|
||||
'char', 'uint8', 'uint16', 'uint32', 'uint64']:
|
||||
return '0'
|
||||
elif type in ['float32', 'float64']:
|
||||
return '0.0'
|
||||
elif type == 'bool':
|
||||
return 'false'
|
||||
|
||||
return ""
|
||||
|
||||
def takes_allocator(type):
|
||||
"""
|
||||
Returns whether or not a type can take an allocator in its constructor. False for all builtin types except string.
|
||||
True for all others.
|
||||
|
||||
@param type: The type
|
||||
@type: str
|
||||
"""
|
||||
return not type in ['byte', 'int8', 'int16', 'int32', 'int64',
|
||||
'char', 'uint8', 'uint16', 'uint32', 'uint64',
|
||||
'float32', 'float64', 'bool', 'time', 'duration']
|
||||
|
||||
def write_initializer_list(s, spec, container_gets_allocator):
|
||||
"""
|
||||
Writes the initializer list for a constructor
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string)
|
||||
should have the allocator passed to its constructor. Assumes the allocator is named _alloc.
|
||||
@type container_gets_allocator: bool
|
||||
"""
|
||||
|
||||
i = 0
|
||||
for field in spec.parsed_fields():
|
||||
if (i == 0):
|
||||
s.write(' : ')
|
||||
else:
|
||||
s.write(' , ')
|
||||
|
||||
val = default_value(field.base_type)
|
||||
use_alloc = takes_allocator(field.base_type)
|
||||
if (field.is_array):
|
||||
if (field.array_len is None and container_gets_allocator):
|
||||
s.write('%s(_alloc)\n'%(field.name))
|
||||
else:
|
||||
s.write('%s()\n'%(field.name))
|
||||
else:
|
||||
if (container_gets_allocator and use_alloc):
|
||||
s.write('%s(_alloc)\n'%(field.name))
|
||||
else:
|
||||
s.write('%s(%s)\n'%(field.name, val))
|
||||
i = i + 1
|
||||
|
||||
def write_fixed_length_assigns(s, spec, container_gets_allocator, cpp_name_prefix):
|
||||
"""
|
||||
Initialize any fixed-length arrays
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param container_gets_allocator: Whether or not a container type (whether it's another message, a vector, array or string)
|
||||
should have the allocator passed to its constructor. Assumes the allocator is named _alloc.
|
||||
@type container_gets_allocator: bool
|
||||
@param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::"
|
||||
@type cpp_name_prefix: str
|
||||
"""
|
||||
# Assign all fixed-length arrays their default values
|
||||
for field in spec.parsed_fields():
|
||||
if (not field.is_array or field.array_len is None):
|
||||
continue
|
||||
|
||||
val = default_value(field.base_type)
|
||||
if (container_gets_allocator and takes_allocator(field.base_type)):
|
||||
# String is a special case, as it is the only builtin type that takes an allocator
|
||||
if (field.base_type == "string"):
|
||||
string_cpp = msg_type_to_cpp("string")
|
||||
s.write(' %s.assign(%s(_alloc));\n'%(field.name, string_cpp))
|
||||
else:
|
||||
(cpp_msg_unqualified, cpp_msg_with_alloc, _) = cpp_message_declarations(cpp_name_prefix, field.base_type)
|
||||
s.write(' %s.assign(%s(_alloc));\n'%(field.name, cpp_msg_with_alloc))
|
||||
elif (len(val) > 0):
|
||||
s.write(' %s.assign(%s);\n'%(field.name, val))
|
||||
|
||||
def write_constructors(s, spec, cpp_name_prefix):
|
||||
"""
|
||||
Writes any necessary constructors for the message
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param cpp_name_prefix: The C++ prefix to use when referring to the message, e.g. "std_msgs::"
|
||||
@type cpp_name_prefix: str
|
||||
"""
|
||||
|
||||
msg = spec.short_name
|
||||
|
||||
# Default constructor
|
||||
s.write(' %s_()\n'%(msg))
|
||||
write_initializer_list(s, spec, False)
|
||||
s.write(' {\n')
|
||||
write_fixed_length_assigns(s, spec, False, cpp_name_prefix)
|
||||
s.write(' }\n\n')
|
||||
|
||||
# Constructor that takes an allocator constructor
|
||||
s.write(' %s_(const ContainerAllocator& _alloc)\n'%(msg))
|
||||
write_initializer_list(s, spec, True)
|
||||
s.write(' {\n')
|
||||
write_fixed_length_assigns(s, spec, True, cpp_name_prefix)
|
||||
s.write(' }\n\n')
|
||||
|
||||
def write_member(s, field):
|
||||
"""
|
||||
Writes a single member's declaration and type typedef
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param type: The member type
|
||||
@type type: str
|
||||
@param name: The name of the member
|
||||
@type name: str
|
||||
"""
|
||||
cpp_type = msg_type_to_cpp(field.type)
|
||||
s.write(' typedef %s _%s_type;\n'%(cpp_type, field.name))
|
||||
s.write(' %s %s;\n\n'%(cpp_type, field.name))
|
||||
|
||||
def write_members(s, spec):
|
||||
"""
|
||||
Write all the member declarations
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
"""
|
||||
[write_member(s, field) for field in spec.parsed_fields()]
|
||||
|
||||
def escape_string(str):
|
||||
str = str.replace('\\', '\\\\')
|
||||
str = str.replace('"', '\\"')
|
||||
return str
|
||||
|
||||
def write_constant_declaration(s, constant):
|
||||
"""
|
||||
Write a constant value as a static member
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param constant: The constant
|
||||
@type constant: roslib.msgs.Constant
|
||||
"""
|
||||
|
||||
# integral types get their declarations as enums to allow use at compile time
|
||||
if (constant.type in ['byte', 'int8', 'int16', 'int32', 'int64', 'char', 'uint8', 'uint16', 'uint32', 'uint64']):
|
||||
s.write(' enum { %s = %s };\n'%(constant.name, constant.val))
|
||||
else:
|
||||
s.write(' static const %s %s;\n'%(msg_type_to_cpp(constant.type), constant.name))
|
||||
|
||||
def write_constant_declarations(s, spec):
|
||||
"""
|
||||
Write all the constants from a spec as static members
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
"""
|
||||
[write_constant_declaration(s, constant) for constant in spec.constants]
|
||||
s.write('\n')
|
||||
|
||||
def write_constant_definition(s, spec, constant):
|
||||
"""
|
||||
Write a constant value as a static member
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param constant: The constant
|
||||
@type constant: roslib.msgs.Constant
|
||||
"""
|
||||
|
||||
# integral types do not need a definition, since they've been defined where they are declared
|
||||
if (constant.type not in ['byte', 'int8', 'int16', 'int32', 'int64', 'char', 'uint8', 'uint16', 'uint32', 'uint64', 'string']):
|
||||
s.write('template<typename ContainerAllocator> const %s %s_<ContainerAllocator>::%s = %s;\n'%(msg_type_to_cpp(constant.type), spec.short_name, constant.name, constant.val))
|
||||
elif (constant.type == 'string'):
|
||||
s.write('template<typename ContainerAllocator> const %s %s_<ContainerAllocator>::%s = "%s";\n'%(msg_type_to_cpp(constant.type), spec.short_name, constant.name, escape_string(constant.val)))
|
||||
|
||||
def write_constant_definitions(s, spec):
|
||||
"""
|
||||
Write all the constants from a spec as static members
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
"""
|
||||
[write_constant_definition(s, spec, constant) for constant in spec.constants]
|
||||
s.write('\n')
|
||||
|
||||
def is_fixed_length(spec):
|
||||
"""
|
||||
Returns whether or not the message is fixed-length
|
||||
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param package: The package of the
|
||||
@type package: str
|
||||
"""
|
||||
types = []
|
||||
for field in spec.parsed_fields():
|
||||
if (field.is_array and field.array_len is None):
|
||||
return False
|
||||
|
||||
if (field.base_type == 'string'):
|
||||
return False
|
||||
|
||||
if (not field.is_builtin):
|
||||
types.append(field.base_type)
|
||||
|
||||
types = set(types)
|
||||
for type in types:
|
||||
type = roslib.msgs.resolve_type(type, spec.package)
|
||||
(_, new_spec) = roslib.msgs.load_by_type(type, spec.package)
|
||||
if (not is_fixed_length(new_spec)):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def write_deprecated_member_functions(s, spec, traits):
|
||||
"""
|
||||
Writes the deprecated member functions for backwards compatibility
|
||||
"""
|
||||
for field in spec.parsed_fields():
|
||||
if (field.is_array):
|
||||
s.write(' ROS_DEPRECATED uint32_t get_%s_size() const { return (uint32_t)%s.size(); }\n'%(field.name, field.name))
|
||||
|
||||
if (field.array_len is None):
|
||||
s.write(' ROS_DEPRECATED void set_%s_size(uint32_t size) { %s.resize((size_t)size); }\n'%(field.name, field.name))
|
||||
s.write(' ROS_DEPRECATED void get_%s_vec(%s& vec) const { vec = this->%s; }\n'%(field.name, msg_type_to_cpp(field.type), field.name))
|
||||
s.write(' ROS_DEPRECATED void set_%s_vec(const %s& vec) { this->%s = vec; }\n'%(field.name, msg_type_to_cpp(field.type), field.name))
|
||||
|
||||
for k, v in traits.items():
|
||||
s.write('private:\n')
|
||||
s.write(' static const char* __s_get%s_() { return "%s"; }\n'%(k, v))
|
||||
s.write('public:\n')
|
||||
s.write(' ROS_DEPRECATED static const std::string __s_get%s() { return __s_get%s_(); }\n\n'%(k, k))
|
||||
s.write(' ROS_DEPRECATED const std::string __get%s() const { return __s_get%s_(); }\n\n'%(k, k))
|
||||
|
||||
s.write(' ROS_DEPRECATED virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t seq) const\n {\n')
|
||||
s.write(' ros::serialization::OStream stream(write_ptr, 1000000000);\n')
|
||||
for field in spec.parsed_fields():
|
||||
s.write(' ros::serialization::serialize(stream, %s);\n'%(field.name))
|
||||
s.write(' return stream.getData();\n }\n\n')
|
||||
|
||||
s.write(' ROS_DEPRECATED virtual uint8_t *deserialize(uint8_t *read_ptr)\n {\n')
|
||||
s.write(' ros::serialization::IStream stream(read_ptr, 1000000000);\n');
|
||||
for field in spec.parsed_fields():
|
||||
s.write(' ros::serialization::deserialize(stream, %s);\n'%(field.name))
|
||||
s.write(' return stream.getData();\n }\n\n')
|
||||
|
||||
s.write(' ROS_DEPRECATED virtual uint32_t serializationLength() const\n {\n')
|
||||
s.write(' uint32_t size = 0;\n');
|
||||
for field in spec.parsed_fields():
|
||||
s.write(' size += ros::serialization::serializationLength(%s);\n'%(field.name))
|
||||
s.write(' return size;\n }\n\n')
|
||||
|
||||
def compute_full_text_escaped(gen_deps_dict):
|
||||
"""
|
||||
Same as roslib.gentools.compute_full_text, except that the
|
||||
resulting text is escaped to be safe for C++ double quotes
|
||||
|
||||
@param get_deps_dict: dictionary returned by get_dependencies call
|
||||
@type get_deps_dict: dict
|
||||
@return: concatenated text for msg/srv file and embedded msg/srv types. Text will be escaped for double quotes
|
||||
@rtype: str
|
||||
"""
|
||||
definition = roslib.gentools.compute_full_text(gen_deps_dict)
|
||||
lines = definition.split('\n')
|
||||
s = StringIO()
|
||||
for line in lines:
|
||||
line = escape_string(line)
|
||||
s.write('%s\\n\\\n'%(line))
|
||||
|
||||
val = s.getvalue()
|
||||
s.close()
|
||||
return val
|
||||
|
||||
def is_hex_string(str):
|
||||
for c in str:
|
||||
if c not in '0123456789abcdefABCDEF':
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def write_trait_char_class(s, class_name, cpp_msg_with_alloc, value, write_static_hex_value = False):
|
||||
"""
|
||||
Writes a class trait for traits which have static value() members that return const char*
|
||||
|
||||
e.g. write_trait_char_class(s, "MD5Sum", "std_msgs::String_<ContainerAllocator>", "hello") yields:
|
||||
template<class ContainerAllocator>
|
||||
struct MD5Sum<std_msgs::String_<ContainerAllocator> >
|
||||
{
|
||||
static const char* value() { return "hello"; }
|
||||
static const char* value(const std_msgs::String_<ContainerAllocator>&) { return value(); }
|
||||
};
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param class_name: The name of the trait class to write
|
||||
@type class_name: str
|
||||
@param cpp_msg_with_alloc: The C++ declaration of the message, including the allocator template
|
||||
@type cpp_msg_with_alloc: str
|
||||
@param value: The value to return in the string
|
||||
@type value: str
|
||||
@param write_static_hex_value: Whether or not to write a set of compile-time-checkable static values. Useful for,
|
||||
for example, MD5Sum. Writes static const uint64_t static_value1... static_valueN
|
||||
@type write_static_hex_value: bool
|
||||
@raise ValueError if write_static_hex_value is True but value contains characters invalid in a hex value
|
||||
"""
|
||||
s.write('template<class ContainerAllocator>\nstruct %s<%s> {\n'%(class_name, cpp_msg_with_alloc))
|
||||
s.write(' static const char* value() \n {\n return "%s";\n }\n\n'%(value))
|
||||
s.write(' static const char* value(const %s&) { return value(); } \n'%(cpp_msg_with_alloc))
|
||||
if (write_static_hex_value):
|
||||
if (not is_hex_string(value)):
|
||||
raise ValueError('%s is not a hex value'%(value))
|
||||
|
||||
iter_count = len(value) / 16
|
||||
for i in range(0, int(iter_count)):
|
||||
start = i*16
|
||||
s.write(' static const uint64_t static_value%s = 0x%sULL;\n'%((i+1), value[start:start+16]))
|
||||
s.write('};\n\n')
|
||||
|
||||
def write_trait_true_class(s, class_name, cpp_msg_with_alloc):
|
||||
"""
|
||||
Writes a true/false trait class
|
||||
|
||||
@param s: stream to write to
|
||||
@type s: stream
|
||||
@param class_name: Name of the trait class
|
||||
@type class_name: str
|
||||
@param cpp_msg_with_alloc: The C++ declaration of the message, including the allocator template
|
||||
@type cpp_msg_with_alloc: str
|
||||
"""
|
||||
s.write('template<class ContainerAllocator> struct %s<%s> : public TrueType {};\n'%(class_name, cpp_msg_with_alloc))
|
||||
|
||||
def write_traits(s, spec, cpp_name_prefix, datatype = None, rospack=None):
|
||||
"""
|
||||
Writes all the traits classes for a message
|
||||
|
||||
@param s: The stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param cpp_name_prefix: The C++ prefix to prepend to a message to refer to it (e.g. "std_msgs::")
|
||||
@type cpp_name_prefix: str
|
||||
@param datatype: The string to write as the datatype of the message. If None (default), pkg/msg is used.
|
||||
@type datatype: str
|
||||
"""
|
||||
# generate dependencies dictionary
|
||||
gendeps_dict = roslib.gentools.get_dependencies(spec, spec.package, compute_files=False, rospack=rospack)
|
||||
md5sum = roslib.gentools.compute_md5(gendeps_dict, rospack=rospack)
|
||||
full_text = compute_full_text_escaped(gendeps_dict)
|
||||
|
||||
if (datatype is None):
|
||||
datatype = '%s'%(spec.full_name)
|
||||
|
||||
(cpp_msg_unqualified, cpp_msg_with_alloc, _) = cpp_message_declarations(cpp_name_prefix, spec.short_name)
|
||||
s.write('namespace ros\n{\n')
|
||||
s.write('namespace message_traits\n{\n')
|
||||
|
||||
write_trait_true_class(s, 'IsMessage', cpp_msg_with_alloc)
|
||||
write_trait_true_class(s, 'IsMessage', cpp_msg_with_alloc + " const")
|
||||
|
||||
write_trait_char_class(s, 'MD5Sum', cpp_msg_with_alloc, md5sum, True)
|
||||
write_trait_char_class(s, 'DataType', cpp_msg_with_alloc, datatype)
|
||||
write_trait_char_class(s, 'Definition', cpp_msg_with_alloc, full_text)
|
||||
|
||||
if (spec.has_header()):
|
||||
write_trait_true_class(s, 'HasHeader', cpp_msg_with_alloc)
|
||||
write_trait_true_class(s, 'HasHeader', ' const' + cpp_msg_with_alloc)
|
||||
|
||||
if (is_fixed_length(spec)):
|
||||
write_trait_true_class(s, 'IsFixedSize', cpp_msg_with_alloc)
|
||||
|
||||
s.write('} // namespace message_traits\n')
|
||||
s.write('} // namespace ros\n\n')
|
||||
|
||||
def write_operations(s, spec, cpp_name_prefix):
|
||||
(cpp_msg_unqualified, cpp_msg_with_alloc, _) = cpp_message_declarations(cpp_name_prefix, spec.short_name)
|
||||
s.write('namespace ros\n{\n')
|
||||
s.write('namespace message_operations\n{\n')
|
||||
|
||||
# Write the Printer operation
|
||||
s.write('\ntemplate<class ContainerAllocator>\nstruct Printer<%s>\n{\n'%(cpp_msg_with_alloc))
|
||||
s.write(' template<typename Stream> static void stream(Stream& s, const std::string& indent, const %s& v) \n {\n'%cpp_msg_with_alloc)
|
||||
for field in spec.parsed_fields():
|
||||
cpp_type = msg_type_to_cpp(field.base_type)
|
||||
if (field.is_array):
|
||||
s.write(' s << indent << "%s[]" << std::endl;\n'%(field.name))
|
||||
s.write(' for (size_t i = 0; i < v.%s.size(); ++i)\n {\n'%(field.name))
|
||||
s.write(' s << indent << " %s[" << i << "]: ";\n'%field.name)
|
||||
indent_increment = ' '
|
||||
if (not field.is_builtin):
|
||||
s.write(' s << std::endl;\n')
|
||||
s.write(' s << indent;\n')
|
||||
indent_increment = ' ';
|
||||
s.write(' Printer<%s>::stream(s, indent + "%s", v.%s[i]);\n'%(cpp_type, indent_increment, field.name))
|
||||
s.write(' }\n')
|
||||
else:
|
||||
s.write(' s << indent << "%s: ";\n'%field.name)
|
||||
indent_increment = ' '
|
||||
if (not field.is_builtin or field.is_array):
|
||||
s.write('s << std::endl;\n')
|
||||
s.write(' Printer<%s>::stream(s, indent + "%s", v.%s);\n'%(cpp_type, indent_increment, field.name))
|
||||
s.write(' }\n')
|
||||
s.write('};\n\n')
|
||||
|
||||
s.write('\n')
|
||||
|
||||
s.write('} // namespace message_operations\n')
|
||||
s.write('} // namespace ros\n\n')
|
||||
|
||||
def write_serialization(s, spec, cpp_name_prefix):
|
||||
"""
|
||||
Writes the Serializer class for a message
|
||||
|
||||
@param s: Stream to write to
|
||||
@type s: stream
|
||||
@param spec: The message spec
|
||||
@type spec: roslib.msgs.MsgSpec
|
||||
@param cpp_name_prefix: The C++ prefix to prepend to a message to refer to it (e.g. "std_msgs::")
|
||||
@type cpp_name_prefix: str
|
||||
"""
|
||||
(cpp_msg_unqualified, cpp_msg_with_alloc, _) = cpp_message_declarations(cpp_name_prefix, spec.short_name)
|
||||
|
||||
s.write('namespace ros\n{\n')
|
||||
s.write('namespace serialization\n{\n\n')
|
||||
|
||||
s.write('template<class ContainerAllocator> struct Serializer<%s>\n{\n'%(cpp_msg_with_alloc))
|
||||
|
||||
s.write(' template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)\n {\n')
|
||||
for field in spec.parsed_fields():
|
||||
s.write(' stream.next(m.%s);\n'%(field.name))
|
||||
s.write(' }\n\n')
|
||||
|
||||
s.write(' ROS_DECLARE_ALLINONE_SERIALIZER\n')
|
||||
|
||||
s.write('}; // struct %s_\n'%(spec.short_name))
|
||||
|
||||
s.write('} // namespace serialization\n')
|
||||
s.write('} // namespace ros\n\n')
|
||||
|
||||
def write_ostream_operator(s, spec, cpp_name_prefix):
|
||||
(cpp_msg_unqualified, cpp_msg_with_alloc, _) = cpp_message_declarations(cpp_name_prefix, spec.short_name)
|
||||
s.write('template<typename ContainerAllocator>\nstd::ostream& operator<<(std::ostream& s, const %s& v)\n{\n'%(cpp_msg_with_alloc))
|
||||
s.write(' ros::message_operations::Printer<%s>::stream(s, "", v);\n return s;}\n\n'%(cpp_msg_with_alloc))
|
||||
|
||||
def generate(msg_path):
|
||||
"""
|
||||
Generate a message
|
||||
|
||||
@param msg_path: The path to the .msg file
|
||||
@type msg_path: str
|
||||
"""
|
||||
(package_dir, package) = roslib.packages.get_dir_pkg(msg_path)
|
||||
(_, spec) = roslib.msgs.load_from_file(msg_path, package)
|
||||
|
||||
s = StringIO()
|
||||
write_begin(s, spec, msg_path)
|
||||
write_generic_includes(s)
|
||||
write_includes(s, spec)
|
||||
|
||||
cpp_prefix = '%s::'%(package)
|
||||
|
||||
s.write('namespace %s\n{\n'%(package))
|
||||
write_struct(s, spec, cpp_prefix)
|
||||
write_constant_definitions(s, spec)
|
||||
write_ostream_operator(s, spec, cpp_prefix)
|
||||
s.write('} // namespace %s\n\n'%(package))
|
||||
|
||||
rospack = RosPack()
|
||||
write_traits(s, spec, cpp_prefix, rospack=rospack)
|
||||
write_serialization(s, spec, cpp_prefix)
|
||||
write_operations(s, spec, cpp_prefix)
|
||||
|
||||
# HACK HACK HACK. The moving of roslib/Header causes many problems. We end up having to make roslib/Header act exactly
|
||||
# like std_msgs/Header (as in, constructor that takes it, as well as operator std_msgs::Header()), and it needs to be
|
||||
# available wherever std_msgs/Header.h has been included
|
||||
if (package == "std_msgs" and spec.short_name == "Header"):
|
||||
s.write("#define STD_MSGS_INCLUDING_HEADER_DEPRECATED_DEF 1\n")
|
||||
s.write("#include <std_msgs/header_deprecated_def.h>\n")
|
||||
s.write("#undef STD_MSGS_INCLUDING_HEADER_DEPRECATED_DEF\n\n")
|
||||
|
||||
write_end(s, spec)
|
||||
|
||||
output_dir = '%s/msg_gen/cpp/include/%s'%(package_dir, package)
|
||||
if (not os.path.exists(output_dir)):
|
||||
# if we're being run concurrently, the above test can report false but os.makedirs can still fail if
|
||||
# another copy just created the directory
|
||||
try:
|
||||
os.makedirs(output_dir)
|
||||
except OSError as e:
|
||||
pass
|
||||
|
||||
f = open('%s/%s.h'%(output_dir, spec.short_name), 'w')
|
||||
f.write(s.getvalue() + "\n")
|
||||
|
||||
s.close()
|
||||
|
||||
def generate_messages(argv):
|
||||
for arg in argv[1:]:
|
||||
generate(arg)
|
||||
|
||||
if __name__ == "__main__":
|
||||
roslib.msgs.set_verbose(False)
|
||||
generate_messages(sys.argv)
|
||||
|
||||
429
thirdparty/ros/ros_comm/clients/roscpp/src/libros/callback_queue.cpp
vendored
Normal file
429
thirdparty/ros/ros_comm/clients/roscpp/src/libros/callback_queue.cpp
vendored
Normal file
@@ -0,0 +1,429 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
// Make sure we use CLOCK_MONOTONIC for the condition variable wait_for if not Apple.
|
||||
#ifndef __APPLE__
|
||||
#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#endif
|
||||
|
||||
#include "ros/callback_queue.h"
|
||||
#include "ros/assert.h"
|
||||
#include <boost/scope_exit.hpp>
|
||||
|
||||
// check if we have really included the backported boost condition variable
|
||||
// just in case someone messes with the include order...
|
||||
#if BOOST_VERSION < 106100
|
||||
#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE
|
||||
#error "needs boost version >= 1.61 or the backported headers!"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
CallbackQueue::CallbackQueue(bool enabled)
|
||||
: calling_(0)
|
||||
, enabled_(enabled)
|
||||
{
|
||||
}
|
||||
|
||||
CallbackQueue::~CallbackQueue()
|
||||
{
|
||||
disable();
|
||||
}
|
||||
|
||||
void CallbackQueue::enable()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
enabled_ = true;
|
||||
|
||||
condition_.notify_all();
|
||||
}
|
||||
|
||||
void CallbackQueue::disable()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
enabled_ = false;
|
||||
|
||||
condition_.notify_all();
|
||||
}
|
||||
|
||||
void CallbackQueue::clear()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
callbacks_.clear();
|
||||
}
|
||||
|
||||
bool CallbackQueue::isEmpty()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
return callbacks_.empty() && calling_ == 0;
|
||||
}
|
||||
|
||||
bool CallbackQueue::isEnabled()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
return enabled_;
|
||||
}
|
||||
|
||||
void CallbackQueue::setupTLS()
|
||||
{
|
||||
if (!tls_.get())
|
||||
{
|
||||
tls_.reset(new TLS);
|
||||
}
|
||||
}
|
||||
|
||||
void CallbackQueue::addCallback(const CallbackInterfacePtr& callback, uint64_t removal_id)
|
||||
{
|
||||
CallbackInfo info;
|
||||
info.callback = callback;
|
||||
info.removal_id = removal_id;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(id_info_mutex_);
|
||||
|
||||
M_IDInfo::iterator it = id_info_.find(removal_id);
|
||||
if (it == id_info_.end())
|
||||
{
|
||||
IDInfoPtr id_info(boost::make_shared<IDInfo>());
|
||||
id_info->id = removal_id;
|
||||
id_info_.insert(std::make_pair(removal_id, id_info));
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if (!enabled_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
callbacks_.push_back(info);
|
||||
}
|
||||
|
||||
condition_.notify_one();
|
||||
}
|
||||
|
||||
CallbackQueue::IDInfoPtr CallbackQueue::getIDInfo(uint64_t id)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(id_info_mutex_);
|
||||
M_IDInfo::iterator it = id_info_.find(id);
|
||||
if (it != id_info_.end())
|
||||
{
|
||||
return it->second;
|
||||
}
|
||||
|
||||
return IDInfoPtr();
|
||||
}
|
||||
|
||||
void CallbackQueue::removeByID(uint64_t removal_id)
|
||||
{
|
||||
setupTLS();
|
||||
|
||||
{
|
||||
IDInfoPtr id_info;
|
||||
{
|
||||
boost::mutex::scoped_lock lock(id_info_mutex_);
|
||||
M_IDInfo::iterator it = id_info_.find(removal_id);
|
||||
if (it != id_info_.end())
|
||||
{
|
||||
id_info = it->second;
|
||||
}
|
||||
else
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// If we're being called from within a callback from our queue, we must unlock the shared lock we already own
|
||||
// here so that we can take a unique lock. We'll re-lock it later.
|
||||
if (tls_->calling_in_this_thread == id_info->id)
|
||||
{
|
||||
id_info->calling_rw_mutex.unlock_shared();
|
||||
}
|
||||
|
||||
{
|
||||
boost::unique_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
D_CallbackInfo::iterator it = callbacks_.begin();
|
||||
for (; it != callbacks_.end();)
|
||||
{
|
||||
CallbackInfo& info = *it;
|
||||
if (info.removal_id == removal_id)
|
||||
{
|
||||
it = callbacks_.erase(it);
|
||||
}
|
||||
else
|
||||
{
|
||||
++it;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (tls_->calling_in_this_thread == id_info->id)
|
||||
{
|
||||
id_info->calling_rw_mutex.lock_shared();
|
||||
}
|
||||
}
|
||||
|
||||
// If we're being called from within a callback, we need to remove the callbacks that match the id that have already been
|
||||
// popped off the queue
|
||||
{
|
||||
D_CallbackInfo::iterator it = tls_->callbacks.begin();
|
||||
D_CallbackInfo::iterator end = tls_->callbacks.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
CallbackInfo& info = *it;
|
||||
if (info.removal_id == removal_id)
|
||||
{
|
||||
info.marked_for_removal = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(id_info_mutex_);
|
||||
id_info_.erase(removal_id);
|
||||
}
|
||||
}
|
||||
|
||||
CallbackQueue::CallOneResult CallbackQueue::callOne(ros::WallDuration timeout)
|
||||
{
|
||||
setupTLS();
|
||||
TLS* tls = tls_.get();
|
||||
|
||||
CallbackInfo cb_info;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if (!enabled_)
|
||||
{
|
||||
return Disabled;
|
||||
}
|
||||
|
||||
if (callbacks_.empty())
|
||||
{
|
||||
if (!timeout.isZero())
|
||||
{
|
||||
condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
|
||||
}
|
||||
|
||||
if (callbacks_.empty())
|
||||
{
|
||||
return Empty;
|
||||
}
|
||||
|
||||
if (!enabled_)
|
||||
{
|
||||
return Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
D_CallbackInfo::iterator it = callbacks_.begin();
|
||||
for (; it != callbacks_.end();)
|
||||
{
|
||||
CallbackInfo& info = *it;
|
||||
|
||||
if (info.marked_for_removal)
|
||||
{
|
||||
it = callbacks_.erase(it);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (info.callback->ready())
|
||||
{
|
||||
cb_info = info;
|
||||
it = callbacks_.erase(it);
|
||||
break;
|
||||
}
|
||||
|
||||
++it;
|
||||
}
|
||||
|
||||
if (!cb_info.callback)
|
||||
{
|
||||
return TryAgain;
|
||||
}
|
||||
|
||||
++calling_;
|
||||
}
|
||||
|
||||
bool was_empty = tls->callbacks.empty();
|
||||
tls->callbacks.push_back(cb_info);
|
||||
if (was_empty)
|
||||
{
|
||||
tls->cb_it = tls->callbacks.begin();
|
||||
}
|
||||
|
||||
CallOneResult res = callOneCB(tls);
|
||||
if (res != Empty)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
--calling_;
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
void CallbackQueue::callAvailable(ros::WallDuration timeout)
|
||||
{
|
||||
setupTLS();
|
||||
TLS* tls = tls_.get();
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if (!enabled_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (callbacks_.empty())
|
||||
{
|
||||
if (!timeout.isZero())
|
||||
{
|
||||
condition_.wait_for(lock, boost::chrono::nanoseconds(timeout.toNSec()));
|
||||
}
|
||||
|
||||
if (callbacks_.empty() || !enabled_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
bool was_empty = tls->callbacks.empty();
|
||||
|
||||
tls->callbacks.insert(tls->callbacks.end(), callbacks_.begin(), callbacks_.end());
|
||||
callbacks_.clear();
|
||||
|
||||
calling_ += tls->callbacks.size();
|
||||
|
||||
if (was_empty)
|
||||
{
|
||||
tls->cb_it = tls->callbacks.begin();
|
||||
}
|
||||
}
|
||||
|
||||
size_t called = 0;
|
||||
|
||||
while (!tls->callbacks.empty())
|
||||
{
|
||||
if (callOneCB(tls) != Empty)
|
||||
{
|
||||
++called;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
calling_ -= called;
|
||||
}
|
||||
}
|
||||
|
||||
CallbackQueue::CallOneResult CallbackQueue::callOneCB(TLS* tls)
|
||||
{
|
||||
// Check for a recursive call. If recursive, increment the current iterator. Otherwise
|
||||
// set the iterator it the beginning of the thread-local callbacks
|
||||
if (tls->calling_in_this_thread == 0xffffffffffffffffULL)
|
||||
{
|
||||
tls->cb_it = tls->callbacks.begin();
|
||||
}
|
||||
|
||||
if (tls->cb_it == tls->callbacks.end())
|
||||
{
|
||||
return Empty;
|
||||
}
|
||||
|
||||
ROS_ASSERT(!tls->callbacks.empty());
|
||||
ROS_ASSERT(tls->cb_it != tls->callbacks.end());
|
||||
|
||||
CallbackInfo info = *tls->cb_it;
|
||||
CallbackInterfacePtr& cb = info.callback;
|
||||
|
||||
IDInfoPtr id_info = getIDInfo(info.removal_id);
|
||||
if (id_info)
|
||||
{
|
||||
boost::shared_lock<boost::shared_mutex> rw_lock(id_info->calling_rw_mutex);
|
||||
|
||||
uint64_t last_calling = tls->calling_in_this_thread;
|
||||
tls->calling_in_this_thread = id_info->id;
|
||||
|
||||
CallbackInterface::CallResult result = CallbackInterface::Invalid;
|
||||
|
||||
{
|
||||
// Ensure that thread id gets restored, even if callback throws.
|
||||
// This is done with RAII rather than try-catch so that the source
|
||||
// of the original exception is not masked in a crash report.
|
||||
BOOST_SCOPE_EXIT(&tls, &last_calling)
|
||||
{
|
||||
tls->calling_in_this_thread = last_calling;
|
||||
}
|
||||
BOOST_SCOPE_EXIT_END
|
||||
|
||||
if (info.marked_for_removal)
|
||||
{
|
||||
tls->cb_it = tls->callbacks.erase(tls->cb_it);
|
||||
}
|
||||
else
|
||||
{
|
||||
tls->cb_it = tls->callbacks.erase(tls->cb_it);
|
||||
result = cb->call();
|
||||
}
|
||||
}
|
||||
|
||||
// Push TryAgain callbacks to the back of the shared queue
|
||||
if (result == CallbackInterface::TryAgain && !info.marked_for_removal)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
callbacks_.push_back(info);
|
||||
|
||||
return TryAgain;
|
||||
}
|
||||
|
||||
return Called;
|
||||
}
|
||||
else
|
||||
{
|
||||
tls->cb_it = tls->callbacks.erase(tls->cb_it);
|
||||
}
|
||||
|
||||
return Called;
|
||||
}
|
||||
|
||||
}
|
||||
61
thirdparty/ros/ros_comm/clients/roscpp/src/libros/common.cpp
vendored
Normal file
61
thirdparty/ros/ros_comm/clients/roscpp/src/libros/common.cpp
vendored
Normal file
@@ -0,0 +1,61 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "ros/common.h"
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <cerrno>
|
||||
#include <cassert>
|
||||
#include <sys/types.h>
|
||||
#if !defined(WIN32)
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
#endif
|
||||
#include <signal.h>
|
||||
|
||||
using std::string;
|
||||
|
||||
void ros::disableAllSignalsInThisThread()
|
||||
{
|
||||
#if !defined(WIN32)
|
||||
// pthreads_win32, despite having an implementation of pthread_sigmask,
|
||||
// doesn't have an implementation of sigset_t, and also doesn't expose its
|
||||
// pthread_sigmask externally.
|
||||
sigset_t signal_set;
|
||||
|
||||
/* block all signals */
|
||||
sigfillset( &signal_set );
|
||||
pthread_sigmask( SIG_BLOCK, &signal_set, NULL );
|
||||
#endif
|
||||
}
|
||||
3
thirdparty/ros/ros_comm/clients/roscpp/src/libros/config.h.in
vendored
Normal file
3
thirdparty/ros/ros_comm/clients/roscpp/src/libros/config.h.in
vendored
Normal file
@@ -0,0 +1,3 @@
|
||||
#cmakedefine HAVE_TRUNC
|
||||
#cmakedefine HAVE_IFADDRS_H
|
||||
#cmakedefine HAVE_EPOLL
|
||||
479
thirdparty/ros/ros_comm/clients/roscpp/src/libros/connection.cpp
vendored
Normal file
479
thirdparty/ros/ros_comm/clients/roscpp/src/libros/connection.cpp
vendored
Normal file
@@ -0,0 +1,479 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "ros/connection.h"
|
||||
#include "ros/transport/transport.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#include <ros/assert.h>
|
||||
|
||||
#include <boost/shared_array.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
Connection::Connection()
|
||||
: is_server_(false)
|
||||
, dropped_(false)
|
||||
, read_filled_(0)
|
||||
, read_size_(0)
|
||||
, reading_(false)
|
||||
, has_read_callback_(0)
|
||||
, write_sent_(0)
|
||||
, write_size_(0)
|
||||
, writing_(false)
|
||||
, has_write_callback_(0)
|
||||
, sending_header_error_(false)
|
||||
{
|
||||
}
|
||||
|
||||
Connection::~Connection()
|
||||
{
|
||||
ROS_DEBUG_NAMED("superdebug", "Connection destructing, dropped=%s", dropped_ ? "true" : "false");
|
||||
|
||||
drop(Destructing);
|
||||
}
|
||||
|
||||
void Connection::initialize(const TransportPtr& transport, bool is_server, const HeaderReceivedFunc& header_func)
|
||||
{
|
||||
ROS_ASSERT(transport);
|
||||
|
||||
transport_ = transport;
|
||||
header_func_ = header_func;
|
||||
is_server_ = is_server;
|
||||
|
||||
transport_->setReadCallback(boost::bind(&Connection::onReadable, this, _1));
|
||||
transport_->setWriteCallback(boost::bind(&Connection::onWriteable, this, _1));
|
||||
transport_->setDisconnectCallback(boost::bind(&Connection::onDisconnect, this, _1));
|
||||
|
||||
if (header_func)
|
||||
{
|
||||
read(4, boost::bind(&Connection::onHeaderLengthRead, this, _1, _2, _3, _4));
|
||||
}
|
||||
}
|
||||
|
||||
boost::signals2::connection Connection::addDropListener(const DropFunc& slot)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
return drop_signal_.connect(slot);
|
||||
}
|
||||
|
||||
void Connection::removeDropListener(const boost::signals2::connection& c)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
c.disconnect();
|
||||
}
|
||||
|
||||
void Connection::onReadable(const TransportPtr& transport)
|
||||
{
|
||||
(void)transport;
|
||||
ROS_ASSERT(transport == transport_);
|
||||
|
||||
readTransport();
|
||||
}
|
||||
|
||||
void Connection::readTransport()
|
||||
{
|
||||
boost::recursive_mutex::scoped_try_lock lock(read_mutex_);
|
||||
|
||||
if (!lock.owns_lock() || dropped_ || reading_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
reading_ = true;
|
||||
|
||||
while (!dropped_ && has_read_callback_)
|
||||
{
|
||||
ROS_ASSERT(read_buffer_);
|
||||
uint32_t to_read = read_size_ - read_filled_;
|
||||
if (to_read > 0)
|
||||
{
|
||||
int32_t bytes_read = transport_->read(read_buffer_.get() + read_filled_, to_read);
|
||||
ROS_DEBUG_NAMED("superdebug", "Connection read %d bytes", bytes_read);
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
else if (bytes_read < 0)
|
||||
{
|
||||
// Bad read, throw away results and report error
|
||||
ReadFinishedFunc callback;
|
||||
callback = read_callback_;
|
||||
read_callback_.clear();
|
||||
read_buffer_.reset();
|
||||
uint32_t size = read_size_;
|
||||
read_size_ = 0;
|
||||
read_filled_ = 0;
|
||||
has_read_callback_ = 0;
|
||||
|
||||
if (callback)
|
||||
{
|
||||
callback(shared_from_this(), read_buffer_, size, false);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
read_filled_ += bytes_read;
|
||||
}
|
||||
|
||||
ROS_ASSERT((int32_t)read_size_ >= 0);
|
||||
ROS_ASSERT((int32_t)read_filled_ >= 0);
|
||||
ROS_ASSERT_MSG(read_filled_ <= read_size_, "read_filled_ = %d, read_size_ = %d", read_filled_, read_size_);
|
||||
|
||||
if (read_filled_ == read_size_ && !dropped_)
|
||||
{
|
||||
ReadFinishedFunc callback;
|
||||
uint32_t size;
|
||||
boost::shared_array<uint8_t> buffer;
|
||||
|
||||
ROS_ASSERT(has_read_callback_);
|
||||
|
||||
// store off the read info in case another read() call is made from within the callback
|
||||
callback = read_callback_;
|
||||
size = read_size_;
|
||||
buffer = read_buffer_;
|
||||
read_callback_.clear();
|
||||
read_buffer_.reset();
|
||||
read_size_ = 0;
|
||||
read_filled_ = 0;
|
||||
has_read_callback_ = 0;
|
||||
|
||||
ROS_DEBUG_NAMED("superdebug", "Calling read callback");
|
||||
callback(shared_from_this(), buffer, size, true);
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!has_read_callback_)
|
||||
{
|
||||
transport_->disableRead();
|
||||
}
|
||||
|
||||
reading_ = false;
|
||||
}
|
||||
|
||||
void Connection::writeTransport()
|
||||
{
|
||||
boost::recursive_mutex::scoped_try_lock lock(write_mutex_);
|
||||
|
||||
if (!lock.owns_lock() || dropped_ || writing_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
writing_ = true;
|
||||
bool can_write_more = true;
|
||||
|
||||
while (has_write_callback_ && can_write_more && !dropped_)
|
||||
{
|
||||
uint32_t to_write = write_size_ - write_sent_;
|
||||
ROS_DEBUG_NAMED("superdebug", "Connection writing %d bytes", to_write);
|
||||
int32_t bytes_sent = transport_->write(write_buffer_.get() + write_sent_, to_write);
|
||||
ROS_DEBUG_NAMED("superdebug", "Connection wrote %d bytes", bytes_sent);
|
||||
|
||||
if (bytes_sent < 0)
|
||||
{
|
||||
writing_ = false;
|
||||
return;
|
||||
}
|
||||
|
||||
write_sent_ += bytes_sent;
|
||||
|
||||
if (bytes_sent < (int)write_size_ - (int)write_sent_)
|
||||
{
|
||||
can_write_more = false;
|
||||
}
|
||||
|
||||
if (write_sent_ == write_size_ && !dropped_)
|
||||
{
|
||||
WriteFinishedFunc callback;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(write_callback_mutex_);
|
||||
ROS_ASSERT(has_write_callback_);
|
||||
// Store off a copy of the callback in case another write() call happens in it
|
||||
callback = write_callback_;
|
||||
write_callback_ = WriteFinishedFunc();
|
||||
write_buffer_ = boost::shared_array<uint8_t>();
|
||||
write_sent_ = 0;
|
||||
write_size_ = 0;
|
||||
has_write_callback_ = 0;
|
||||
}
|
||||
|
||||
ROS_DEBUG_NAMED("superdebug", "Calling write callback");
|
||||
callback(shared_from_this());
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(write_callback_mutex_);
|
||||
if (!has_write_callback_)
|
||||
{
|
||||
transport_->disableWrite();
|
||||
}
|
||||
}
|
||||
|
||||
writing_ = false;
|
||||
}
|
||||
|
||||
void Connection::onWriteable(const TransportPtr& transport)
|
||||
{
|
||||
(void)transport;
|
||||
ROS_ASSERT(transport == transport_);
|
||||
|
||||
writeTransport();
|
||||
}
|
||||
|
||||
void Connection::read(uint32_t size, const ReadFinishedFunc& callback)
|
||||
{
|
||||
if (dropped_ || sending_header_error_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(read_mutex_);
|
||||
|
||||
ROS_ASSERT(!read_callback_);
|
||||
|
||||
read_callback_ = callback;
|
||||
read_buffer_ = boost::shared_array<uint8_t>(new uint8_t[size]);
|
||||
read_size_ = size;
|
||||
read_filled_ = 0;
|
||||
has_read_callback_ = 1;
|
||||
}
|
||||
|
||||
transport_->enableRead();
|
||||
|
||||
// read immediately if possible
|
||||
readTransport();
|
||||
}
|
||||
|
||||
void Connection::write(const boost::shared_array<uint8_t>& buffer, uint32_t size, const WriteFinishedFunc& callback, bool immediate)
|
||||
{
|
||||
if (dropped_ || sending_header_error_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(write_callback_mutex_);
|
||||
|
||||
ROS_ASSERT(!write_callback_);
|
||||
|
||||
write_callback_ = callback;
|
||||
write_buffer_ = buffer;
|
||||
write_size_ = size;
|
||||
write_sent_ = 0;
|
||||
has_write_callback_ = 1;
|
||||
}
|
||||
|
||||
transport_->enableWrite();
|
||||
|
||||
if (immediate)
|
||||
{
|
||||
// write immediately if possible
|
||||
writeTransport();
|
||||
}
|
||||
}
|
||||
|
||||
void Connection::onDisconnect(const TransportPtr& transport)
|
||||
{
|
||||
(void)transport;
|
||||
ROS_ASSERT(transport == transport_);
|
||||
|
||||
drop(TransportDisconnect);
|
||||
}
|
||||
|
||||
void Connection::drop(DropReason reason)
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Connection::drop(%u)", reason);
|
||||
bool did_drop = false;
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
if (!dropped_)
|
||||
{
|
||||
dropped_ = true;
|
||||
did_drop = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (did_drop)
|
||||
{
|
||||
drop_signal_(shared_from_this(), reason);
|
||||
transport_->close();
|
||||
}
|
||||
}
|
||||
|
||||
bool Connection::isDropped()
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
return dropped_;
|
||||
}
|
||||
|
||||
void Connection::writeHeader(const M_string& key_vals, const WriteFinishedFunc& finished_callback)
|
||||
{
|
||||
ROS_ASSERT(!header_written_callback_);
|
||||
header_written_callback_ = finished_callback;
|
||||
|
||||
if (!transport_->requiresHeader())
|
||||
{
|
||||
onHeaderWritten(shared_from_this());
|
||||
return;
|
||||
}
|
||||
|
||||
boost::shared_array<uint8_t> buffer;
|
||||
uint32_t len;
|
||||
Header::write(key_vals, buffer, len);
|
||||
|
||||
uint32_t msg_len = len + 4;
|
||||
boost::shared_array<uint8_t> full_msg(new uint8_t[msg_len]);
|
||||
memcpy(full_msg.get() + 4, buffer.get(), len);
|
||||
*((uint32_t*)full_msg.get()) = len;
|
||||
|
||||
write(full_msg, msg_len, boost::bind(&Connection::onHeaderWritten, this, _1), false);
|
||||
}
|
||||
|
||||
void Connection::sendHeaderError(const std::string& error_msg)
|
||||
{
|
||||
M_string m;
|
||||
m["error"] = error_msg;
|
||||
|
||||
writeHeader(m, boost::bind(&Connection::onErrorHeaderWritten, this, _1));
|
||||
sending_header_error_ = true;
|
||||
}
|
||||
|
||||
void Connection::onHeaderLengthRead(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
|
||||
{
|
||||
(void)size;
|
||||
ROS_ASSERT(conn.get() == this);
|
||||
ROS_ASSERT(size == 4);
|
||||
|
||||
if (!success)
|
||||
return;
|
||||
|
||||
uint32_t len = *((uint32_t*)buffer.get());
|
||||
|
||||
if (len > 1000000000)
|
||||
{
|
||||
ROS_ERROR("a header of over a gigabyte was " \
|
||||
"predicted in tcpros. that seems highly " \
|
||||
"unlikely, so I'll assume protocol " \
|
||||
"synchronization is lost.");
|
||||
conn->drop(HeaderError);
|
||||
}
|
||||
|
||||
read(len, boost::bind(&Connection::onHeaderRead, this, _1, _2, _3, _4));
|
||||
}
|
||||
|
||||
void Connection::onHeaderRead(const ConnectionPtr& conn, const boost::shared_array<uint8_t>& buffer, uint32_t size, bool success)
|
||||
{
|
||||
ROS_ASSERT(conn.get() == this);
|
||||
|
||||
if (!success)
|
||||
return;
|
||||
|
||||
std::string error_msg;
|
||||
if (!header_.parse(buffer, size, error_msg))
|
||||
{
|
||||
drop(HeaderError);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::string error_val;
|
||||
if (header_.getValue("error", error_val))
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Received error message in header for connection to [%s]: [%s]", transport_->getTransportInfo().c_str(), error_val.c_str());
|
||||
drop(HeaderError);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ASSERT(header_func_);
|
||||
|
||||
transport_->parseHeader(header_);
|
||||
|
||||
header_func_(conn, header_);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Connection::onHeaderWritten(const ConnectionPtr& conn)
|
||||
{
|
||||
ROS_ASSERT(conn.get() == this);
|
||||
ROS_ASSERT(header_written_callback_);
|
||||
|
||||
header_written_callback_(conn);
|
||||
header_written_callback_ = WriteFinishedFunc();
|
||||
}
|
||||
|
||||
void Connection::onErrorHeaderWritten(const ConnectionPtr& conn)
|
||||
{
|
||||
(void)conn;
|
||||
drop(HeaderError);
|
||||
}
|
||||
|
||||
void Connection::setHeaderReceivedCallback(const HeaderReceivedFunc& func)
|
||||
{
|
||||
header_func_ = func;
|
||||
|
||||
if (transport_->requiresHeader())
|
||||
read(4, boost::bind(&Connection::onHeaderLengthRead, this, _1, _2, _3, _4));
|
||||
}
|
||||
|
||||
std::string Connection::getCallerId()
|
||||
{
|
||||
std::string callerid;
|
||||
if (header_.getValue("callerid", callerid))
|
||||
{
|
||||
return callerid;
|
||||
}
|
||||
|
||||
return std::string("unknown");
|
||||
}
|
||||
|
||||
std::string Connection::getRemoteString()
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "callerid=[" << getCallerId() << "] address=[" << transport_->getTransportInfo() << "]";
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
}
|
||||
228
thirdparty/ros/ros_comm/clients/roscpp/src/libros/connection_manager.cpp
vendored
Normal file
228
thirdparty/ros/ros_comm/clients/roscpp/src/libros/connection_manager.cpp
vendored
Normal file
@@ -0,0 +1,228 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#include "ros/connection_manager.h"
|
||||
#include "ros/poll_manager.h"
|
||||
#include "ros/connection.h"
|
||||
#include "ros/transport_subscriber_link.h"
|
||||
#include "ros/service_client_link.h"
|
||||
#include "ros/transport/transport_tcp.h"
|
||||
#include "ros/transport/transport_udp.h"
|
||||
#include "ros/file_log.h"
|
||||
#include "ros/network.h"
|
||||
|
||||
#include <ros/assert.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
const ConnectionManagerPtr& ConnectionManager::instance()
|
||||
{
|
||||
static ConnectionManagerPtr connection_manager = boost::make_shared<ConnectionManager>();
|
||||
return connection_manager;
|
||||
}
|
||||
|
||||
ConnectionManager::ConnectionManager()
|
||||
: connection_id_counter_(0)
|
||||
{
|
||||
}
|
||||
|
||||
ConnectionManager::~ConnectionManager()
|
||||
{
|
||||
shutdown();
|
||||
}
|
||||
|
||||
void ConnectionManager::start()
|
||||
{
|
||||
poll_manager_ = PollManager::instance();
|
||||
poll_conn_ = poll_manager_->addPollThreadListener(boost::bind(&ConnectionManager::removeDroppedConnections,
|
||||
this));
|
||||
|
||||
// Bring up the TCP listener socket
|
||||
tcpserver_transport_ = boost::make_shared<TransportTCP>(&poll_manager_->getPollSet());
|
||||
if (!tcpserver_transport_->listen(network::getTCPROSPort(),
|
||||
MAX_TCPROS_CONN_QUEUE,
|
||||
boost::bind(&ConnectionManager::tcprosAcceptConnection, this, _1)))
|
||||
{
|
||||
ROS_FATAL("Listen on port [%d] failed", network::getTCPROSPort());
|
||||
ROS_BREAK();
|
||||
}
|
||||
|
||||
// Bring up the UDP listener socket
|
||||
udpserver_transport_ = boost::make_shared<TransportUDP>(&poll_manager_->getPollSet());
|
||||
if (!udpserver_transport_->createIncoming(0, true))
|
||||
{
|
||||
ROS_FATAL("Listen failed");
|
||||
ROS_BREAK();
|
||||
}
|
||||
}
|
||||
|
||||
void ConnectionManager::shutdown()
|
||||
{
|
||||
if (udpserver_transport_)
|
||||
{
|
||||
udpserver_transport_->close();
|
||||
udpserver_transport_.reset();
|
||||
}
|
||||
|
||||
if (tcpserver_transport_)
|
||||
{
|
||||
tcpserver_transport_->close();
|
||||
tcpserver_transport_.reset();
|
||||
}
|
||||
|
||||
poll_manager_->removePollThreadListener(poll_conn_);
|
||||
|
||||
clear(Connection::Destructing);
|
||||
}
|
||||
|
||||
void ConnectionManager::clear(Connection::DropReason reason)
|
||||
{
|
||||
S_Connection local_connections;
|
||||
{
|
||||
boost::mutex::scoped_lock conn_lock(connections_mutex_);
|
||||
local_connections.swap(connections_);
|
||||
}
|
||||
|
||||
for(S_Connection::iterator itr = local_connections.begin();
|
||||
itr != local_connections.end();
|
||||
itr++)
|
||||
{
|
||||
const ConnectionPtr& conn = *itr;
|
||||
conn->drop(reason);
|
||||
}
|
||||
|
||||
boost::mutex::scoped_lock dropped_lock(dropped_connections_mutex_);
|
||||
dropped_connections_.clear();
|
||||
}
|
||||
|
||||
uint32_t ConnectionManager::getTCPPort()
|
||||
{
|
||||
return tcpserver_transport_->getServerPort();
|
||||
}
|
||||
|
||||
uint32_t ConnectionManager::getUDPPort()
|
||||
{
|
||||
return udpserver_transport_->getServerPort();
|
||||
}
|
||||
|
||||
uint32_t ConnectionManager::getNewConnectionID()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(connection_id_counter_mutex_);
|
||||
uint32_t ret = connection_id_counter_++;
|
||||
return ret;
|
||||
}
|
||||
|
||||
void ConnectionManager::addConnection(const ConnectionPtr& conn)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(connections_mutex_);
|
||||
|
||||
connections_.insert(conn);
|
||||
conn->addDropListener(boost::bind(&ConnectionManager::onConnectionDropped, this, _1));
|
||||
}
|
||||
|
||||
void ConnectionManager::onConnectionDropped(const ConnectionPtr& conn)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(dropped_connections_mutex_);
|
||||
dropped_connections_.push_back(conn);
|
||||
}
|
||||
|
||||
void ConnectionManager::removeDroppedConnections()
|
||||
{
|
||||
V_Connection local_dropped;
|
||||
{
|
||||
boost::mutex::scoped_lock dropped_lock(dropped_connections_mutex_);
|
||||
dropped_connections_.swap(local_dropped);
|
||||
}
|
||||
|
||||
boost::mutex::scoped_lock conn_lock(connections_mutex_);
|
||||
|
||||
V_Connection::iterator conn_it = local_dropped.begin();
|
||||
V_Connection::iterator conn_end = local_dropped.end();
|
||||
for (;conn_it != conn_end; ++conn_it)
|
||||
{
|
||||
const ConnectionPtr& conn = *conn_it;
|
||||
connections_.erase(conn);
|
||||
}
|
||||
}
|
||||
|
||||
void ConnectionManager::udprosIncomingConnection(const TransportUDPPtr& transport, Header& header)
|
||||
{
|
||||
std::string client_uri = ""; // TODO: transport->getClientURI();
|
||||
ROSCPP_LOG_DEBUG("UDPROS received a connection from [%s]", client_uri.c_str());
|
||||
|
||||
ConnectionPtr conn(boost::make_shared<Connection>());
|
||||
addConnection(conn);
|
||||
|
||||
conn->initialize(transport, true, NULL);
|
||||
onConnectionHeaderReceived(conn, header);
|
||||
}
|
||||
|
||||
void ConnectionManager::tcprosAcceptConnection(const TransportTCPPtr& transport)
|
||||
{
|
||||
std::string client_uri = transport->getClientURI();
|
||||
ROSCPP_LOG_DEBUG("TCPROS received a connection from [%s]", client_uri.c_str());
|
||||
|
||||
ConnectionPtr conn(boost::make_shared<Connection>());
|
||||
addConnection(conn);
|
||||
|
||||
conn->initialize(transport, true, boost::bind(&ConnectionManager::onConnectionHeaderReceived, this, _1, _2));
|
||||
}
|
||||
|
||||
bool ConnectionManager::onConnectionHeaderReceived(const ConnectionPtr& conn, const Header& header)
|
||||
{
|
||||
bool ret = false;
|
||||
std::string val;
|
||||
if (header.getValue("topic", val))
|
||||
{
|
||||
ROSCPP_CONN_LOG_DEBUG("Connection: Creating TransportSubscriberLink for topic [%s] connected to [%s]",
|
||||
val.c_str(), conn->getRemoteString().c_str());
|
||||
|
||||
TransportSubscriberLinkPtr sub_link(boost::make_shared<TransportSubscriberLink>());
|
||||
sub_link->initialize(conn);
|
||||
ret = sub_link->handleHeader(header);
|
||||
}
|
||||
else if (header.getValue("service", val))
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Connection: Creating ServiceClientLink for service [%s] connected to [%s]",
|
||||
val.c_str(), conn->getRemoteString().c_str());
|
||||
|
||||
ServiceClientLinkPtr link(boost::make_shared<ServiceClientLink>());
|
||||
link->initialize(conn);
|
||||
ret = link->handleHeader(header);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Got a connection for a type other than 'topic' or 'service' from [%s]. Fail.",
|
||||
conn->getRemoteString().c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
}
|
||||
126
thirdparty/ros/ros_comm/clients/roscpp/src/libros/file_log.cpp
vendored
Normal file
126
thirdparty/ros/ros_comm/clients/roscpp/src/libros/file_log.cpp
vendored
Normal file
@@ -0,0 +1,126 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include <cstdio>
|
||||
#include "ros/file_log.h"
|
||||
#include "ros/this_node.h"
|
||||
|
||||
#include <ros/io.h>
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <boost/filesystem.hpp>
|
||||
|
||||
#ifdef _MSC_VER
|
||||
#ifdef snprintf
|
||||
#undef snprintf
|
||||
#endif
|
||||
#define snprintf _snprintf_s
|
||||
#endif
|
||||
|
||||
|
||||
namespace fs = boost::filesystem;
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
namespace file_log
|
||||
{
|
||||
|
||||
std::string g_log_directory;
|
||||
|
||||
const std::string& getLogDirectory()
|
||||
{
|
||||
return g_log_directory;
|
||||
}
|
||||
|
||||
void init(const M_string& remappings)
|
||||
{
|
||||
std::string log_file_name;
|
||||
M_string::const_iterator it = remappings.find("__log");
|
||||
if (it != remappings.end())
|
||||
{
|
||||
log_file_name = it->second;
|
||||
}
|
||||
|
||||
{
|
||||
// Log filename can be specified on the command line through __log
|
||||
// If it's been set, don't create our own name
|
||||
if (log_file_name.empty())
|
||||
{
|
||||
// Setup the logfile appender
|
||||
// Can't do this in rosconsole because the node name is not known
|
||||
pid_t pid = getpid();
|
||||
std::string ros_log_env;
|
||||
if ( get_environment_variable(ros_log_env, "ROS_LOG_DIR"))
|
||||
{
|
||||
log_file_name = ros_log_env + std::string("/");
|
||||
}
|
||||
else
|
||||
{
|
||||
if ( get_environment_variable(ros_log_env, "ROS_HOME"))
|
||||
{
|
||||
log_file_name = ros_log_env + std::string("/log/");
|
||||
}
|
||||
else
|
||||
{
|
||||
// Not cross-platform?
|
||||
if( get_environment_variable(ros_log_env, "HOME") )
|
||||
{
|
||||
std::string dotros = ros_log_env + std::string("/.ros/");
|
||||
fs::create_directory(dotros);
|
||||
log_file_name = dotros + "log/";
|
||||
fs::create_directory(log_file_name);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// sanitize the node name and tack it to the filename
|
||||
for (size_t i = 1; i < this_node::getName().length(); i++)
|
||||
{
|
||||
if (!isalnum(this_node::getName()[i]))
|
||||
{
|
||||
log_file_name += '_';
|
||||
}
|
||||
else
|
||||
{
|
||||
log_file_name += this_node::getName()[i];
|
||||
}
|
||||
}
|
||||
|
||||
char pid_str[100];
|
||||
snprintf(pid_str, sizeof(pid_str), "%d", pid);
|
||||
log_file_name += std::string("_") + std::string(pid_str) + std::string(".log");
|
||||
}
|
||||
|
||||
log_file_name = fs::system_complete(log_file_name).string();
|
||||
g_log_directory = fs::path(log_file_name).parent_path().string();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace file_log
|
||||
|
||||
} // namespace ros
|
||||
612
thirdparty/ros/ros_comm/clients/roscpp/src/libros/init.cpp
vendored
Normal file
612
thirdparty/ros/ros_comm/clients/roscpp/src/libros/init.cpp
vendored
Normal file
@@ -0,0 +1,612 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "ros/init.h"
|
||||
#include "ros/names.h"
|
||||
#include "ros/xmlrpc_manager.h"
|
||||
#include "ros/poll_manager.h"
|
||||
#include "ros/connection_manager.h"
|
||||
#include "ros/topic_manager.h"
|
||||
#include "ros/service_manager.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/network.h"
|
||||
#include "ros/file_log.h"
|
||||
#include "ros/callback_queue.h"
|
||||
#include "ros/param.h"
|
||||
#include "ros/rosout_appender.h"
|
||||
#include "ros/subscribe_options.h"
|
||||
#include "ros/transport/transport_tcp.h"
|
||||
#include "ros/internal_timer_manager.h"
|
||||
#include "xmlrpcpp/XmlRpcSocket.h"
|
||||
|
||||
#include "roscpp/GetLoggers.h"
|
||||
#include "roscpp/SetLoggerLevel.h"
|
||||
#include "roscpp/Empty.h"
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <ros/time.h>
|
||||
#include <rosgraph_msgs/Clock.h>
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
#include <signal.h>
|
||||
|
||||
#include <cstdlib>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
namespace master
|
||||
{
|
||||
void init(const M_string& remappings);
|
||||
}
|
||||
|
||||
namespace this_node
|
||||
{
|
||||
void init(const std::string& names, const M_string& remappings, uint32_t options);
|
||||
}
|
||||
|
||||
namespace network
|
||||
{
|
||||
void init(const M_string& remappings);
|
||||
}
|
||||
|
||||
namespace param
|
||||
{
|
||||
void init(const M_string& remappings);
|
||||
}
|
||||
|
||||
namespace file_log
|
||||
{
|
||||
void init(const M_string& remappings);
|
||||
}
|
||||
|
||||
CallbackQueuePtr g_global_queue;
|
||||
ROSOutAppender* g_rosout_appender;
|
||||
static CallbackQueuePtr g_internal_callback_queue;
|
||||
|
||||
static bool g_initialized = false;
|
||||
static bool g_started = false;
|
||||
static bool g_atexit_registered = false;
|
||||
static boost::mutex g_start_mutex;
|
||||
static bool g_ok = false;
|
||||
static uint32_t g_init_options = 0;
|
||||
static bool g_shutdown_requested = false;
|
||||
static volatile bool g_shutting_down = false;
|
||||
static boost::recursive_mutex g_shutting_down_mutex;
|
||||
static boost::thread g_internal_queue_thread;
|
||||
|
||||
bool isInitialized()
|
||||
{
|
||||
return g_initialized;
|
||||
}
|
||||
|
||||
bool isShuttingDown()
|
||||
{
|
||||
return g_shutting_down;
|
||||
}
|
||||
|
||||
void checkForShutdown()
|
||||
{
|
||||
if (g_shutdown_requested)
|
||||
{
|
||||
// Since this gets run from within a mutex inside PollManager, we need to prevent ourselves from deadlocking with
|
||||
// another thread that's already in the middle of shutdown()
|
||||
boost::recursive_mutex::scoped_try_lock lock(g_shutting_down_mutex, boost::defer_lock);
|
||||
while (!lock.try_lock() && !g_shutting_down)
|
||||
{
|
||||
ros::WallDuration(0.001).sleep();
|
||||
}
|
||||
|
||||
if (!g_shutting_down)
|
||||
{
|
||||
shutdown();
|
||||
}
|
||||
|
||||
g_shutdown_requested = false;
|
||||
}
|
||||
}
|
||||
|
||||
void requestShutdown()
|
||||
{
|
||||
g_shutdown_requested = true;
|
||||
}
|
||||
|
||||
void atexitCallback()
|
||||
{
|
||||
if (ok() && !isShuttingDown())
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("shutting down due to exit() or end of main() without cleanup of all NodeHandles");
|
||||
g_started = false; // don't shutdown singletons, because they are already destroyed
|
||||
shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
void shutdownCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
|
||||
{
|
||||
int num_params = 0;
|
||||
if (params.getType() == XmlRpc::XmlRpcValue::TypeArray)
|
||||
num_params = params.size();
|
||||
if (num_params > 1)
|
||||
{
|
||||
std::string reason = params[1];
|
||||
ROS_WARN("Shutdown request received.");
|
||||
ROS_WARN("Reason given for shutdown: [%s]", reason.c_str());
|
||||
requestShutdown();
|
||||
}
|
||||
|
||||
result = xmlrpc::responseInt(1, "", 0);
|
||||
}
|
||||
|
||||
bool getLoggers(roscpp::GetLoggers::Request&, roscpp::GetLoggers::Response& resp)
|
||||
{
|
||||
std::map<std::string, ros::console::levels::Level> loggers;
|
||||
bool success = ::ros::console::get_loggers(loggers);
|
||||
if (success)
|
||||
{
|
||||
for (std::map<std::string, ros::console::levels::Level>::const_iterator it = loggers.begin(); it != loggers.end(); it++)
|
||||
{
|
||||
roscpp::Logger logger;
|
||||
logger.name = it->first;
|
||||
ros::console::levels::Level level = it->second;
|
||||
if (level == ros::console::levels::Debug)
|
||||
{
|
||||
logger.level = "debug";
|
||||
}
|
||||
else if (level == ros::console::levels::Info)
|
||||
{
|
||||
logger.level = "info";
|
||||
}
|
||||
else if (level == ros::console::levels::Warn)
|
||||
{
|
||||
logger.level = "warn";
|
||||
}
|
||||
else if (level == ros::console::levels::Error)
|
||||
{
|
||||
logger.level = "error";
|
||||
}
|
||||
else if (level == ros::console::levels::Fatal)
|
||||
{
|
||||
logger.level = "fatal";
|
||||
}
|
||||
resp.loggers.push_back(logger);
|
||||
}
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
bool setLoggerLevel(roscpp::SetLoggerLevel::Request& req, roscpp::SetLoggerLevel::Response&)
|
||||
{
|
||||
std::transform(req.level.begin(), req.level.end(), req.level.begin(), (int(*)(int))std::toupper);
|
||||
|
||||
ros::console::levels::Level level;
|
||||
if (req.level == "DEBUG")
|
||||
{
|
||||
level = ros::console::levels::Debug;
|
||||
}
|
||||
else if (req.level == "INFO")
|
||||
{
|
||||
level = ros::console::levels::Info;
|
||||
}
|
||||
else if (req.level == "WARN")
|
||||
{
|
||||
level = ros::console::levels::Warn;
|
||||
}
|
||||
else if (req.level == "ERROR")
|
||||
{
|
||||
level = ros::console::levels::Error;
|
||||
}
|
||||
else if (req.level == "FATAL")
|
||||
{
|
||||
level = ros::console::levels::Fatal;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
bool success = ::ros::console::set_logger_level(req.logger, level);
|
||||
if (success)
|
||||
{
|
||||
console::notifyLoggerLevelsChanged();
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool closeAllConnections(roscpp::Empty::Request&, roscpp::Empty::Response&)
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("close_all_connections service called, closing connections");
|
||||
ConnectionManager::instance()->clear(Connection::TransportDisconnect);
|
||||
return true;
|
||||
}
|
||||
|
||||
void clockCallback(const rosgraph_msgs::Clock::ConstPtr& msg)
|
||||
{
|
||||
Time::setNow(msg->clock);
|
||||
}
|
||||
|
||||
CallbackQueuePtr getInternalCallbackQueue()
|
||||
{
|
||||
if (!g_internal_callback_queue)
|
||||
{
|
||||
g_internal_callback_queue.reset(new CallbackQueue);
|
||||
}
|
||||
|
||||
return g_internal_callback_queue;
|
||||
}
|
||||
|
||||
void basicSigintHandler(int sig)
|
||||
{
|
||||
(void)sig;
|
||||
ros::requestShutdown();
|
||||
}
|
||||
|
||||
void internalCallbackQueueThreadFunc()
|
||||
{
|
||||
disableAllSignalsInThisThread();
|
||||
|
||||
CallbackQueuePtr queue = getInternalCallbackQueue();
|
||||
|
||||
while (!g_shutting_down)
|
||||
{
|
||||
queue->callAvailable(WallDuration(0.1));
|
||||
}
|
||||
}
|
||||
|
||||
bool isStarted()
|
||||
{
|
||||
return g_started;
|
||||
}
|
||||
|
||||
void start()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(g_start_mutex);
|
||||
if (g_started)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
g_shutdown_requested = false;
|
||||
g_shutting_down = false;
|
||||
g_started = true;
|
||||
g_ok = true;
|
||||
|
||||
bool enable_debug = false;
|
||||
std::string enable_debug_env;
|
||||
if ( get_environment_variable(enable_debug_env,"ROSCPP_ENABLE_DEBUG") )
|
||||
{
|
||||
try
|
||||
{
|
||||
enable_debug = boost::lexical_cast<bool>(enable_debug_env.c_str());
|
||||
}
|
||||
catch (boost::bad_lexical_cast&)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef _MSC_VER
|
||||
if (env_ipv6)
|
||||
{
|
||||
free(env_ipv6);
|
||||
}
|
||||
#endif
|
||||
|
||||
param::param("/tcp_keepalive", TransportTCP::s_use_keepalive_, TransportTCP::s_use_keepalive_);
|
||||
|
||||
PollManager::instance()->addPollThreadListener(checkForShutdown);
|
||||
XMLRPCManager::instance()->bind("shutdown", shutdownCallback);
|
||||
|
||||
initInternalTimerManager();
|
||||
|
||||
TopicManager::instance()->start();
|
||||
ServiceManager::instance()->start();
|
||||
ConnectionManager::instance()->start();
|
||||
PollManager::instance()->start();
|
||||
XMLRPCManager::instance()->start();
|
||||
|
||||
if (!(g_init_options & init_options::NoSigintHandler))
|
||||
{
|
||||
signal(SIGINT, basicSigintHandler);
|
||||
}
|
||||
|
||||
ros::Time::init();
|
||||
|
||||
if (!(g_init_options & init_options::NoRosout))
|
||||
{
|
||||
g_rosout_appender = new ROSOutAppender;
|
||||
ros::console::register_appender(g_rosout_appender);
|
||||
}
|
||||
|
||||
if (g_shutting_down) goto end;
|
||||
|
||||
{
|
||||
ros::AdvertiseServiceOptions ops;
|
||||
ops.init<roscpp::GetLoggers>(names::resolve("~get_loggers"), getLoggers);
|
||||
ops.callback_queue = getInternalCallbackQueue().get();
|
||||
ServiceManager::instance()->advertiseService(ops);
|
||||
}
|
||||
|
||||
if (g_shutting_down) goto end;
|
||||
|
||||
{
|
||||
ros::AdvertiseServiceOptions ops;
|
||||
ops.init<roscpp::SetLoggerLevel>(names::resolve("~set_logger_level"), setLoggerLevel);
|
||||
ops.callback_queue = getInternalCallbackQueue().get();
|
||||
ServiceManager::instance()->advertiseService(ops);
|
||||
}
|
||||
|
||||
if (g_shutting_down) goto end;
|
||||
|
||||
if (enable_debug)
|
||||
{
|
||||
ros::AdvertiseServiceOptions ops;
|
||||
ops.init<roscpp::Empty>(names::resolve("~debug/close_all_connections"), closeAllConnections);
|
||||
ops.callback_queue = getInternalCallbackQueue().get();
|
||||
ServiceManager::instance()->advertiseService(ops);
|
||||
}
|
||||
|
||||
if (g_shutting_down) goto end;
|
||||
|
||||
{
|
||||
bool use_sim_time = false;
|
||||
param::param("/use_sim_time", use_sim_time, use_sim_time);
|
||||
|
||||
if (use_sim_time)
|
||||
{
|
||||
Time::setNow(ros::Time());
|
||||
}
|
||||
|
||||
if (g_shutting_down) goto end;
|
||||
|
||||
if (use_sim_time)
|
||||
{
|
||||
ros::SubscribeOptions ops;
|
||||
ops.init<rosgraph_msgs::Clock>(names::resolve("/clock"), 1, clockCallback);
|
||||
ops.callback_queue = getInternalCallbackQueue().get();
|
||||
TopicManager::instance()->subscribe(ops);
|
||||
}
|
||||
}
|
||||
|
||||
if (g_shutting_down) goto end;
|
||||
|
||||
g_internal_queue_thread = boost::thread(internalCallbackQueueThreadFunc);
|
||||
getGlobalCallbackQueue()->enable();
|
||||
|
||||
ROSCPP_LOG_DEBUG("Started node [%s], pid [%d], bound on [%s], xmlrpc port [%d], tcpros port [%d], using [%s] time",
|
||||
this_node::getName().c_str(), getpid(), network::getHost().c_str(),
|
||||
XMLRPCManager::instance()->getServerPort(), ConnectionManager::instance()->getTCPPort(),
|
||||
Time::useSystemTime() ? "real" : "sim");
|
||||
|
||||
// Label used to abort if we've started shutting down in the middle of start(), which can happen in
|
||||
// threaded code or if Ctrl-C is pressed while we're initializing
|
||||
end:
|
||||
// If we received a shutdown request while initializing, wait until we've shutdown to continue
|
||||
if (g_shutting_down)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
|
||||
}
|
||||
}
|
||||
|
||||
void check_ipv6_environment() {
|
||||
char* env_ipv6 = NULL;
|
||||
#ifdef _MSC_VER
|
||||
_dupenv_s(&env_ipv6, NULL, "ROS_IPV6");
|
||||
#else
|
||||
env_ipv6 = getenv("ROS_IPV6");
|
||||
#endif
|
||||
|
||||
bool use_ipv6 = (env_ipv6 && strcmp(env_ipv6,"on") == 0);
|
||||
TransportTCP::s_use_ipv6_ = use_ipv6;
|
||||
XmlRpc::XmlRpcSocket::s_use_ipv6_ = use_ipv6;
|
||||
}
|
||||
|
||||
void init(const M_string& remappings, const std::string& name, uint32_t options)
|
||||
{
|
||||
if (!g_atexit_registered)
|
||||
{
|
||||
g_atexit_registered = true;
|
||||
atexit(atexitCallback);
|
||||
}
|
||||
|
||||
if (!g_global_queue)
|
||||
{
|
||||
g_global_queue.reset(new CallbackQueue);
|
||||
}
|
||||
|
||||
if (!g_initialized)
|
||||
{
|
||||
g_init_options = options;
|
||||
g_ok = true;
|
||||
|
||||
ROSCONSOLE_AUTOINIT;
|
||||
// Disable SIGPIPE
|
||||
#ifndef WIN32
|
||||
signal(SIGPIPE, SIG_IGN);
|
||||
#endif
|
||||
check_ipv6_environment();
|
||||
network::init(remappings);
|
||||
master::init(remappings);
|
||||
// names:: namespace is initialized by this_node
|
||||
this_node::init(name, remappings, options);
|
||||
file_log::init(remappings);
|
||||
param::init(remappings);
|
||||
|
||||
g_initialized = true;
|
||||
}
|
||||
}
|
||||
|
||||
void init(int& argc, char** argv, const std::string& name, uint32_t options)
|
||||
{
|
||||
M_string remappings;
|
||||
|
||||
int full_argc = argc;
|
||||
// now, move the remapping argv's to the end, and decrement argc as needed
|
||||
for (int i = 0; i < argc; )
|
||||
{
|
||||
std::string arg = argv[i];
|
||||
size_t pos = arg.find(":=");
|
||||
if (pos != std::string::npos)
|
||||
{
|
||||
std::string local_name = arg.substr(0, pos);
|
||||
std::string external_name = arg.substr(pos + 2);
|
||||
|
||||
ROSCPP_LOG_DEBUG("remap: %s => %s", local_name.c_str(), external_name.c_str());
|
||||
remappings[local_name] = external_name;
|
||||
|
||||
// shuffle everybody down and stuff this guy at the end of argv
|
||||
char *tmp = argv[i];
|
||||
for (int j = i; j < full_argc - 1; j++)
|
||||
argv[j] = argv[j+1];
|
||||
argv[argc-1] = tmp;
|
||||
argc--;
|
||||
}
|
||||
else
|
||||
{
|
||||
i++; // move on, since we didn't shuffle anybody here to replace it
|
||||
}
|
||||
}
|
||||
|
||||
init(remappings, name, options);
|
||||
}
|
||||
|
||||
void init(const VP_string& remappings, const std::string& name, uint32_t options)
|
||||
{
|
||||
M_string remappings_map;
|
||||
VP_string::const_iterator it = remappings.begin();
|
||||
VP_string::const_iterator end = remappings.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
remappings_map[it->first] = it->second;
|
||||
}
|
||||
|
||||
init(remappings_map, name, options);
|
||||
}
|
||||
|
||||
std::string getROSArg(int argc, const char* const* argv, const std::string& arg)
|
||||
{
|
||||
for (int i = 0; i < argc; ++i)
|
||||
{
|
||||
std::string str_arg = argv[i];
|
||||
size_t pos = str_arg.find(":=");
|
||||
if (str_arg.substr(0,pos) == arg)
|
||||
{
|
||||
return str_arg.substr(pos+2);
|
||||
}
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
void removeROSArgs(int argc, const char* const* argv, V_string& args_out)
|
||||
{
|
||||
for (int i = 0; i < argc; ++i)
|
||||
{
|
||||
std::string arg = argv[i];
|
||||
size_t pos = arg.find(":=");
|
||||
if (pos == std::string::npos)
|
||||
{
|
||||
args_out.push_back(arg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void spin()
|
||||
{
|
||||
SingleThreadedSpinner s;
|
||||
spin(s);
|
||||
}
|
||||
|
||||
void spin(Spinner& s)
|
||||
{
|
||||
s.spin();
|
||||
}
|
||||
|
||||
void spinOnce()
|
||||
{
|
||||
g_global_queue->callAvailable(ros::WallDuration());
|
||||
}
|
||||
|
||||
void waitForShutdown()
|
||||
{
|
||||
while (ok())
|
||||
{
|
||||
WallDuration(0.05).sleep();
|
||||
}
|
||||
}
|
||||
|
||||
CallbackQueue* getGlobalCallbackQueue()
|
||||
{
|
||||
return g_global_queue.get();
|
||||
}
|
||||
|
||||
bool ok()
|
||||
{
|
||||
return g_ok;
|
||||
}
|
||||
|
||||
void shutdown()
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(g_shutting_down_mutex);
|
||||
if (g_shutting_down)
|
||||
return;
|
||||
else
|
||||
g_shutting_down = true;
|
||||
|
||||
ros::console::shutdown();
|
||||
|
||||
g_global_queue->disable();
|
||||
g_global_queue->clear();
|
||||
|
||||
if (g_internal_queue_thread.get_id() != boost::this_thread::get_id())
|
||||
{
|
||||
g_internal_queue_thread.join();
|
||||
}
|
||||
|
||||
g_rosout_appender = 0;
|
||||
|
||||
if (g_started)
|
||||
{
|
||||
TopicManager::instance()->shutdown();
|
||||
ServiceManager::instance()->shutdown();
|
||||
PollManager::instance()->shutdown();
|
||||
ConnectionManager::instance()->shutdown();
|
||||
XMLRPCManager::instance()->shutdown();
|
||||
}
|
||||
|
||||
g_started = false;
|
||||
g_ok = false;
|
||||
Time::shutdown();
|
||||
}
|
||||
|
||||
}
|
||||
62
thirdparty/ros/ros_comm/clients/roscpp/src/libros/internal_timer_manager.cpp
vendored
Normal file
62
thirdparty/ros/ros_comm/clients/roscpp/src/libros/internal_timer_manager.cpp
vendored
Normal file
@@ -0,0 +1,62 @@
|
||||
/*
|
||||
* Copyright (C) 2010, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
// Make sure we use CLOCK_MONOTONIC for the condition variable if not Apple.
|
||||
#ifndef __APPLE__
|
||||
#define BOOST_THREAD_HAS_CONDATTR_SET_CLOCK_MONOTONIC
|
||||
#endif
|
||||
|
||||
#include "ros/timer_manager.h"
|
||||
#include "ros/internal_timer_manager.h"
|
||||
|
||||
// check if we have really included the backported boost condition variable
|
||||
// just in case someone messes with the include order...
|
||||
#if BOOST_VERSION < 106100
|
||||
#ifndef USING_BACKPORTED_BOOST_CONDITION_VARIABLE
|
||||
#error "steady timer needs boost version >= 1.61 or the backported headers!"
|
||||
#endif
|
||||
#endif
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
static InternalTimerManagerPtr g_timer_manager;
|
||||
|
||||
InternalTimerManagerPtr getInternalTimerManager()
|
||||
{
|
||||
return g_timer_manager;
|
||||
}
|
||||
|
||||
void initInternalTimerManager()
|
||||
{
|
||||
if (!g_timer_manager)
|
||||
{
|
||||
g_timer_manager.reset(new InternalTimerManager);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
159
thirdparty/ros/ros_comm/clients/roscpp/src/libros/intraprocess_publisher_link.cpp
vendored
Normal file
159
thirdparty/ros/ros_comm/clients/roscpp/src/libros/intraprocess_publisher_link.cpp
vendored
Normal file
@@ -0,0 +1,159 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "ros/intraprocess_publisher_link.h"
|
||||
#include "ros/intraprocess_subscriber_link.h"
|
||||
#include "ros/subscription.h"
|
||||
#include "ros/header.h"
|
||||
#include "ros/connection.h"
|
||||
#include "ros/transport/transport.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/connection_manager.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <sstream>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
IntraProcessPublisherLink::IntraProcessPublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri, const TransportHints& transport_hints)
|
||||
: PublisherLink(parent, xmlrpc_uri, transport_hints)
|
||||
, dropped_(false)
|
||||
{
|
||||
}
|
||||
|
||||
IntraProcessPublisherLink::~IntraProcessPublisherLink()
|
||||
{
|
||||
}
|
||||
|
||||
void IntraProcessPublisherLink::setPublisher(const IntraProcessSubscriberLinkPtr& publisher)
|
||||
{
|
||||
publisher_ = publisher;
|
||||
|
||||
SubscriptionPtr parent = parent_.lock();
|
||||
ROS_ASSERT(parent);
|
||||
|
||||
Header header;
|
||||
M_stringPtr values = header.getValues();
|
||||
(*values)["callerid"] = this_node::getName();
|
||||
(*values)["topic"] = parent->getName();
|
||||
(*values)["type"] = publisher->getDataType();
|
||||
(*values)["md5sum"] = publisher->getMD5Sum();
|
||||
(*values)["message_definition"] = publisher->getMessageDefinition();
|
||||
(*values)["latching"] = publisher->isLatching() ? "1" : "0";
|
||||
setHeader(header);
|
||||
}
|
||||
|
||||
void IntraProcessPublisherLink::drop()
|
||||
{
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
dropped_ = true;
|
||||
}
|
||||
|
||||
if (publisher_)
|
||||
{
|
||||
publisher_->drop();
|
||||
publisher_.reset();
|
||||
}
|
||||
|
||||
if (SubscriptionPtr parent = parent_.lock())
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Connection to local publisher on topic [%s] dropped", parent->getName().c_str());
|
||||
|
||||
parent->removePublisherLink(shared_from_this());
|
||||
}
|
||||
}
|
||||
|
||||
void IntraProcessPublisherLink::handleMessage(const SerializedMessage& m, bool ser, bool nocopy)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
stats_.bytes_received_ += m.num_bytes;
|
||||
stats_.messages_received_++;
|
||||
|
||||
SubscriptionPtr parent = parent_.lock();
|
||||
|
||||
if (parent)
|
||||
{
|
||||
stats_.drops_ += parent->handleMessage(m, ser, nocopy, header_.getValues(), shared_from_this());
|
||||
}
|
||||
}
|
||||
|
||||
std::string IntraProcessPublisherLink::getTransportType()
|
||||
{
|
||||
return std::string("INTRAPROCESS");
|
||||
}
|
||||
|
||||
std::string IntraProcessPublisherLink::getTransportInfo()
|
||||
{
|
||||
// TODO: Check if we can dump more useful information here
|
||||
return getTransportType();
|
||||
}
|
||||
|
||||
void IntraProcessPublisherLink::getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
if (dropped_)
|
||||
{
|
||||
ser = false;
|
||||
nocopy = false;
|
||||
return;
|
||||
}
|
||||
|
||||
SubscriptionPtr parent = parent_.lock();
|
||||
if (parent)
|
||||
{
|
||||
parent->getPublishTypes(ser, nocopy, ti);
|
||||
}
|
||||
else
|
||||
{
|
||||
ser = true;
|
||||
nocopy = false;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
|
||||
134
thirdparty/ros/ros_comm/clients/roscpp/src/libros/intraprocess_subscriber_link.cpp
vendored
Normal file
134
thirdparty/ros/ros_comm/clients/roscpp/src/libros/intraprocess_subscriber_link.cpp
vendored
Normal file
@@ -0,0 +1,134 @@
|
||||
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include "ros/intraprocess_subscriber_link.h"
|
||||
#include "ros/intraprocess_publisher_link.h"
|
||||
#include "ros/publication.h"
|
||||
#include "ros/header.h"
|
||||
#include "ros/connection.h"
|
||||
#include "ros/transport/transport.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/connection_manager.h"
|
||||
#include "ros/topic_manager.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
IntraProcessSubscriberLink::IntraProcessSubscriberLink(const PublicationPtr& parent)
|
||||
: dropped_(false)
|
||||
{
|
||||
ROS_ASSERT(parent);
|
||||
parent_ = parent;
|
||||
topic_ = parent->getName();
|
||||
}
|
||||
|
||||
IntraProcessSubscriberLink::~IntraProcessSubscriberLink()
|
||||
{
|
||||
}
|
||||
|
||||
void IntraProcessSubscriberLink::setSubscriber(const IntraProcessPublisherLinkPtr& subscriber)
|
||||
{
|
||||
subscriber_ = subscriber;
|
||||
connection_id_ = ConnectionManager::instance()->getNewConnectionID();
|
||||
destination_caller_id_ = this_node::getName();
|
||||
}
|
||||
|
||||
bool IntraProcessSubscriberLink::isLatching()
|
||||
{
|
||||
if (PublicationPtr parent = parent_.lock())
|
||||
{
|
||||
return parent->isLatching();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void IntraProcessSubscriberLink::enqueueMessage(const SerializedMessage& m, bool ser, bool nocopy)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
ROS_ASSERT(subscriber_);
|
||||
subscriber_->handleMessage(m, ser, nocopy);
|
||||
}
|
||||
|
||||
std::string IntraProcessSubscriberLink::getTransportType()
|
||||
{
|
||||
return std::string("INTRAPROCESS");
|
||||
}
|
||||
|
||||
std::string IntraProcessSubscriberLink::getTransportInfo()
|
||||
{
|
||||
// TODO: Check if we can dump more useful information here
|
||||
return getTransportType();
|
||||
}
|
||||
|
||||
void IntraProcessSubscriberLink::drop()
|
||||
{
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
dropped_ = true;
|
||||
}
|
||||
|
||||
if (subscriber_)
|
||||
{
|
||||
subscriber_->drop();
|
||||
subscriber_.reset();
|
||||
}
|
||||
|
||||
if (PublicationPtr parent = parent_.lock())
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("Connection to local subscriber on topic [%s] dropped", topic_.c_str());
|
||||
|
||||
parent->removeSubscriberLink(shared_from_this());
|
||||
}
|
||||
}
|
||||
|
||||
void IntraProcessSubscriberLink::getPublishTypes(bool& ser, bool& nocopy, const std::type_info& ti)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(drop_mutex_);
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
subscriber_->getPublishTypes(ser, nocopy, ti);
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
529
thirdparty/ros/ros_comm/clients/roscpp/src/libros/io.cpp
vendored
Normal file
529
thirdparty/ros/ros_comm/clients/roscpp/src/libros/io.cpp
vendored
Normal file
@@ -0,0 +1,529 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
/*****************************************************************************
|
||||
** Includes
|
||||
*****************************************************************************/
|
||||
|
||||
#include "config.h"
|
||||
|
||||
#include <ros/io.h>
|
||||
#include <ros/assert.h> // don't need if we dont call the pipe functions.
|
||||
#include <errno.h> // for EFAULT and co.
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#ifdef WIN32
|
||||
#else
|
||||
#include <cstring> // strerror
|
||||
#include <fcntl.h> // for non-blocking configuration
|
||||
#endif
|
||||
|
||||
#ifdef HAVE_EPOLL
|
||||
#include <sys/epoll.h>
|
||||
#endif
|
||||
|
||||
/*****************************************************************************
|
||||
** Macros
|
||||
*****************************************************************************/
|
||||
|
||||
#define UNUSED(expr) do { (void)(expr); } while (0)
|
||||
|
||||
/*****************************************************************************
|
||||
** Namespaces
|
||||
*****************************************************************************/
|
||||
|
||||
namespace ros {
|
||||
|
||||
int last_socket_error() {
|
||||
#ifdef WIN32
|
||||
return WSAGetLastError();
|
||||
#else
|
||||
return errno;
|
||||
#endif
|
||||
}
|
||||
const char* last_socket_error_string() {
|
||||
#ifdef WIN32
|
||||
// could fix this to use FORMAT_MESSAGE and print a real string later,
|
||||
// but not high priority.
|
||||
std::stringstream ostream;
|
||||
ostream << "WSA Error: " << WSAGetLastError();
|
||||
return ostream.str().c_str();
|
||||
#else
|
||||
return strerror(errno);
|
||||
#endif
|
||||
}
|
||||
|
||||
bool last_socket_error_is_would_block() {
|
||||
#if defined(WIN32)
|
||||
if ( WSAGetLastError() == WSAEWOULDBLOCK ) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
#else
|
||||
if ( ( errno == EAGAIN ) || ( errno == EWOULDBLOCK ) ) { // posix permits either
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
int create_socket_watcher() {
|
||||
int epfd = -1;
|
||||
#if defined(HAVE_EPOLL)
|
||||
epfd = ::epoll_create1(0);
|
||||
if (epfd < 0)
|
||||
{
|
||||
ROS_ERROR("Unable to create epoll watcher: %s", strerror(errno));
|
||||
}
|
||||
#endif
|
||||
return epfd;
|
||||
}
|
||||
|
||||
void close_socket_watcher(int fd) {
|
||||
if (fd >= 0)
|
||||
::close(fd);
|
||||
}
|
||||
|
||||
void add_socket_to_watcher(int epfd, int fd) {
|
||||
#if defined(HAVE_EPOLL)
|
||||
struct epoll_event ev;
|
||||
bzero(&ev, sizeof(ev));
|
||||
|
||||
ev.events = 0;
|
||||
ev.data.fd = fd;
|
||||
|
||||
if (::epoll_ctl(epfd, EPOLL_CTL_ADD, fd, &ev))
|
||||
{
|
||||
ROS_ERROR("Unable to add FD to epoll: %s", strerror(errno));
|
||||
}
|
||||
#else
|
||||
UNUSED(epfd);
|
||||
UNUSED(fd);
|
||||
#endif
|
||||
}
|
||||
|
||||
void del_socket_from_watcher(int epfd, int fd) {
|
||||
#if defined(HAVE_EPOLL)
|
||||
if (::epoll_ctl(epfd, EPOLL_CTL_DEL, fd, NULL))
|
||||
{
|
||||
ROS_ERROR("Unable to remove FD to epoll: %s", strerror(errno));
|
||||
}
|
||||
#else
|
||||
UNUSED(epfd);
|
||||
UNUSED(fd);
|
||||
#endif
|
||||
}
|
||||
|
||||
void set_events_on_socket(int epfd, int fd, int events) {
|
||||
#if defined(HAVE_EPOLL)
|
||||
struct epoll_event ev;
|
||||
bzero(&ev, sizeof(ev));
|
||||
|
||||
ev.events = events;
|
||||
ev.data.fd = fd;
|
||||
if (::epoll_ctl(epfd, EPOLL_CTL_MOD, fd, &ev))
|
||||
{
|
||||
ROS_ERROR("Unable to modify FD epoll: %s", strerror(errno));
|
||||
}
|
||||
#else
|
||||
UNUSED(epfd);
|
||||
UNUSED(fd);
|
||||
UNUSED(events);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*****************************************************************************
|
||||
** Service Robotics/Libssh Functions
|
||||
*****************************************************************************/
|
||||
/**
|
||||
* @brief A cross platform polling function for sockets.
|
||||
*
|
||||
* Windows doesn't have a polling function until Vista (WSAPoll) and even then
|
||||
* its implementation is not supposed to be great. This works for a broader
|
||||
* range of platforms and will suffice.
|
||||
* @param epfd - the socket watcher to poll on.
|
||||
* @param fds - the socket set (descriptor's and events) to poll for.
|
||||
* @param nfds - the number of descriptors to poll for.
|
||||
* @param timeout - timeout in milliseconds.
|
||||
* @return pollfd_vector_ptr : NULL on error, empty on timeout, a list of structures with received events.
|
||||
*/
|
||||
pollfd_vector_ptr poll_sockets(int epfd, socket_pollfd *fds, nfds_t nfds, int timeout) {
|
||||
#if defined(WIN32)
|
||||
fd_set readfds, writefds, exceptfds;
|
||||
struct timeval tv, *ptv;
|
||||
socket_fd_t max_fd;
|
||||
int rc;
|
||||
nfds_t i;
|
||||
boost::shared_ptr<std::vector<socket_pollfd> > ofds;
|
||||
|
||||
UNUSED(epfd);
|
||||
|
||||
if (fds == NULL) {
|
||||
errno = EFAULT;
|
||||
return ofds;
|
||||
}
|
||||
|
||||
FD_ZERO (&readfds);
|
||||
FD_ZERO (&writefds);
|
||||
FD_ZERO (&exceptfds);
|
||||
|
||||
/*********************
|
||||
** Compute fd sets
|
||||
**********************/
|
||||
// also find the largest descriptor.
|
||||
for (rc = -1, max_fd = 0, i = 0; i < nfds; i++) {
|
||||
if (fds[i].fd == INVALID_SOCKET) {
|
||||
continue;
|
||||
}
|
||||
if (fds[i].events & (POLLIN | POLLRDNORM)) {
|
||||
FD_SET (fds[i].fd, &readfds);
|
||||
}
|
||||
if (fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND)) {
|
||||
FD_SET (fds[i].fd, &writefds);
|
||||
}
|
||||
if (fds[i].events & (POLLPRI | POLLRDBAND)) {
|
||||
FD_SET (fds[i].fd, &exceptfds);
|
||||
}
|
||||
if (fds[i].fd > max_fd &&
|
||||
(fds[i].events & (POLLIN | POLLOUT | POLLPRI |
|
||||
POLLRDNORM | POLLRDBAND |
|
||||
POLLWRNORM | POLLWRBAND))) {
|
||||
max_fd = fds[i].fd;
|
||||
rc = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (rc == -1) {
|
||||
errno = EINVAL;
|
||||
return ofds;
|
||||
}
|
||||
/*********************
|
||||
** Setting the timeout
|
||||
**********************/
|
||||
if (timeout < 0) {
|
||||
ptv = NULL;
|
||||
} else {
|
||||
ptv = &tv;
|
||||
if (timeout == 0) {
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = 0;
|
||||
} else {
|
||||
tv.tv_sec = timeout / 1000;
|
||||
tv.tv_usec = (timeout % 1000) * 1000;
|
||||
}
|
||||
}
|
||||
|
||||
rc = select (max_fd + 1, &readfds, &writefds, &exceptfds, ptv);
|
||||
if (rc < 0) {
|
||||
return ofds;
|
||||
}
|
||||
ofds.reset(new std::vector<socket_pollfd>);
|
||||
if ( rc == 0 ) {
|
||||
return ofds;
|
||||
}
|
||||
|
||||
for (rc = 0, i = 0; i < nfds; i++) {
|
||||
if (fds[i].fd != INVALID_SOCKET) {
|
||||
fds[i].revents = 0;
|
||||
|
||||
if (FD_ISSET(fds[i].fd, &readfds)) {
|
||||
int save_errno = errno;
|
||||
char data[64] = {0};
|
||||
int ret;
|
||||
|
||||
/* support for POLLHUP */
|
||||
// just check if there's incoming data, without removing it from the queue.
|
||||
ret = recv(fds[i].fd, data, 64, MSG_PEEK);
|
||||
#ifdef WIN32
|
||||
if ((ret == -1) &&
|
||||
(errno == WSAESHUTDOWN || errno == WSAECONNRESET ||
|
||||
(errno == WSAECONNABORTED) || errno == WSAENETRESET))
|
||||
#else
|
||||
if ((ret == -1) &&
|
||||
(errno == ESHUTDOWN || errno == ECONNRESET ||
|
||||
(errno == ECONNABORTED) || errno == ENETRESET))
|
||||
#endif
|
||||
{
|
||||
fds[i].revents |= POLLHUP;
|
||||
} else {
|
||||
fds[i].revents |= fds[i].events & (POLLIN | POLLRDNORM);
|
||||
}
|
||||
errno = save_errno;
|
||||
}
|
||||
if (FD_ISSET(fds[i].fd, &writefds)) {
|
||||
fds[i].revents |= fds[i].events & (POLLOUT | POLLWRNORM | POLLWRBAND);
|
||||
}
|
||||
|
||||
if (FD_ISSET(fds[i].fd, &exceptfds)) {
|
||||
fds[i].revents |= fds[i].events & (POLLPRI | POLLRDBAND);
|
||||
}
|
||||
|
||||
if (fds[i].revents & ~POLLHUP) {
|
||||
rc++;
|
||||
}
|
||||
} else {
|
||||
fds[i].revents = POLLNVAL;
|
||||
}
|
||||
ofds->push_back(fds[i]);
|
||||
}
|
||||
return ofds;
|
||||
#elif defined (HAVE_EPOLL)
|
||||
UNUSED(nfds);
|
||||
UNUSED(fds);
|
||||
struct epoll_event ev[nfds];
|
||||
pollfd_vector_ptr ofds;
|
||||
|
||||
int fd_cnt = ::epoll_wait(epfd, ev, nfds, timeout);
|
||||
|
||||
if (fd_cnt < 0)
|
||||
{
|
||||
// EINTR means that we got interrupted by a signal, and is not an error
|
||||
if(errno != EINTR) {
|
||||
ROS_ERROR("Error in epoll_wait! %s", strerror(errno));
|
||||
ofds.reset();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ofds.reset(new std::vector<socket_pollfd>);
|
||||
for (int i = 0; i < fd_cnt; i++)
|
||||
{
|
||||
socket_pollfd pfd;
|
||||
pfd.fd = ev[i].data.fd;
|
||||
pfd.revents = ev[i].events;
|
||||
ofds->push_back(pfd);
|
||||
}
|
||||
}
|
||||
return ofds;
|
||||
#else
|
||||
UNUSED(epfd);
|
||||
pollfd_vector_ptr ofds(new std::vector<socket_pollfd>);
|
||||
// Clear the `revents` fields
|
||||
for (nfds_t i = 0; i < nfds; i++)
|
||||
{
|
||||
fds[i].revents = 0;
|
||||
}
|
||||
|
||||
// use an existing poll implementation
|
||||
int result = poll(fds, nfds, timeout);
|
||||
if ( result < 0 )
|
||||
{
|
||||
// EINTR means that we got interrupted by a signal, and is not an error
|
||||
if(errno != EINTR)
|
||||
{
|
||||
ROS_ERROR("Error in poll! %s", strerror(errno));
|
||||
ofds.reset();
|
||||
}
|
||||
} else {
|
||||
for (nfds_t i = 0; i < nfds; i++)
|
||||
{
|
||||
if (fds[i].revents)
|
||||
{
|
||||
ofds->push_back(fds[i]);
|
||||
fds[i].revents = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
return ofds;
|
||||
#endif // poll_sockets functions
|
||||
}
|
||||
/*****************************************************************************
|
||||
** Socket Utilities
|
||||
*****************************************************************************/
|
||||
/**
|
||||
* Sets the socket as non blocking.
|
||||
* @return int : 0 on success, WSAGetLastError()/errno on failure.
|
||||
*/
|
||||
int set_non_blocking(socket_fd_t &socket) {
|
||||
#ifdef WIN32
|
||||
u_long non_blocking = 1;
|
||||
if(ioctlsocket( socket, FIONBIO, &non_blocking ) != 0 )
|
||||
{
|
||||
return WSAGetLastError();
|
||||
}
|
||||
#else
|
||||
if(fcntl(socket, F_SETFL, O_NONBLOCK) == -1)
|
||||
{
|
||||
return errno;
|
||||
}
|
||||
#endif
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Close the socket.
|
||||
*
|
||||
* @return int : 0 on success, -1 on failure.
|
||||
*/
|
||||
int close_socket(socket_fd_t &socket) {
|
||||
#ifdef WIN32
|
||||
if(::closesocket(socket) == SOCKET_ERROR ) {
|
||||
return -1;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
#else
|
||||
if (::close(socket) < 0) {
|
||||
return -1;
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
#endif //WIN32
|
||||
}
|
||||
|
||||
/*****************************************************************************
|
||||
** Signal Pair
|
||||
*****************************************************************************/
|
||||
/**
|
||||
* This code is primarily from the msdn socket tutorials.
|
||||
* @param signal_pair : a pair of sockets linked to each other over localhost.
|
||||
* @return 0 on success, -1 on failure.
|
||||
*/
|
||||
int create_signal_pair(signal_fd_t signal_pair[2]) {
|
||||
#ifdef WIN32 // use a socket pair
|
||||
signal_pair[0] = INVALID_SOCKET;
|
||||
signal_pair[1] = INVALID_SOCKET;
|
||||
|
||||
union {
|
||||
struct sockaddr_in inaddr;
|
||||
struct sockaddr addr;
|
||||
} a;
|
||||
socklen_t addrlen = sizeof(a.inaddr);
|
||||
|
||||
/*********************
|
||||
** Listen Socket
|
||||
**********************/
|
||||
socket_fd_t listen_socket = INVALID_SOCKET;
|
||||
listen_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
|
||||
if (listen_socket == INVALID_SOCKET) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
// allow it to be bound to an address already in use - do we actually need this?
|
||||
int reuse = 1;
|
||||
if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, (char*) &reuse, (socklen_t) sizeof(reuse)) == SOCKET_ERROR ) {
|
||||
::closesocket(listen_socket);
|
||||
return -1;
|
||||
}
|
||||
|
||||
memset(&a, 0, sizeof(a));
|
||||
a.inaddr.sin_family = AF_INET;
|
||||
a.inaddr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
|
||||
// For TCP/IP, if the port is specified as zero, the service provider assigns
|
||||
// a unique port to the application from the dynamic client port range.
|
||||
a.inaddr.sin_port = 0;
|
||||
|
||||
if (bind(listen_socket, &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
|
||||
::closesocket(listen_socket);
|
||||
return -1;
|
||||
}
|
||||
// we need this below because the system auto filled in some entries, e.g. port #
|
||||
if (getsockname(listen_socket, &a.addr, &addrlen) == SOCKET_ERROR) {
|
||||
::closesocket(listen_socket);
|
||||
return -1;
|
||||
}
|
||||
// max 1 connection permitted
|
||||
if (listen(listen_socket, 1) == SOCKET_ERROR) {
|
||||
::closesocket(listen_socket);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*********************
|
||||
** Connection
|
||||
**********************/
|
||||
// do we need io overlapping?
|
||||
// DWORD flags = (make_overlapped ? WSA_FLAG_OVERLAPPED : 0);
|
||||
DWORD overlapped_flag = 0;
|
||||
signal_pair[0] = WSASocket(AF_INET, SOCK_STREAM, 0, NULL, 0, overlapped_flag);
|
||||
if (signal_pair[0] == INVALID_SOCKET) {
|
||||
::closesocket(listen_socket);
|
||||
::closesocket(signal_pair[0]);
|
||||
return -1;
|
||||
}
|
||||
// reusing the information from above to connect to the listener
|
||||
if (connect(signal_pair[0], &a.addr, sizeof(a.inaddr)) == SOCKET_ERROR) {
|
||||
::closesocket(listen_socket);
|
||||
::closesocket(signal_pair[0]);
|
||||
return -1;
|
||||
}
|
||||
/*********************
|
||||
** Accept
|
||||
**********************/
|
||||
signal_pair[1] = accept(listen_socket, NULL, NULL);
|
||||
if (signal_pair[1] == INVALID_SOCKET) {
|
||||
::closesocket(listen_socket);
|
||||
::closesocket(signal_pair[0]);
|
||||
::closesocket(signal_pair[1]);
|
||||
return -1;
|
||||
}
|
||||
/*********************
|
||||
** Nonblocking
|
||||
**********************/
|
||||
// should we do this or should we set io overlapping?
|
||||
if ( (set_non_blocking(signal_pair[0]) != 0) || (set_non_blocking(signal_pair[1]) != 0) ) {
|
||||
::closesocket(listen_socket);
|
||||
::closesocket(signal_pair[0]);
|
||||
::closesocket(signal_pair[1]);
|
||||
return -1;
|
||||
}
|
||||
/*********************
|
||||
** Cleanup
|
||||
**********************/
|
||||
::closesocket(listen_socket); // the listener has done its job.
|
||||
return 0;
|
||||
#else // use a pipe pair
|
||||
// initialize
|
||||
signal_pair[0] = -1;
|
||||
signal_pair[1] = -1;
|
||||
|
||||
if(pipe(signal_pair) != 0) {
|
||||
ROS_FATAL( "pipe() failed");
|
||||
return -1;
|
||||
}
|
||||
if(fcntl(signal_pair[0], F_SETFL, O_NONBLOCK) == -1) {
|
||||
ROS_FATAL( "fcntl() failed");
|
||||
return -1;
|
||||
}
|
||||
if(fcntl(signal_pair[1], F_SETFL, O_NONBLOCK) == -1) {
|
||||
ROS_FATAL( "fcntl() failed");
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
#endif // create_pipe
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
253
thirdparty/ros/ros_comm/clients/roscpp/src/libros/master.cpp
vendored
Normal file
253
thirdparty/ros/ros_comm/clients/roscpp/src/libros/master.cpp
vendored
Normal file
@@ -0,0 +1,253 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#include "ros/master.h"
|
||||
#include "ros/xmlrpc_manager.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/init.h"
|
||||
#include "ros/network.h"
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <ros/assert.h>
|
||||
|
||||
#include "xmlrpcpp/XmlRpc.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
namespace master
|
||||
{
|
||||
|
||||
uint32_t g_port = 0;
|
||||
std::string g_host;
|
||||
std::string g_uri;
|
||||
ros::WallDuration g_retry_timeout;
|
||||
|
||||
void init(const M_string& remappings)
|
||||
{
|
||||
M_string::const_iterator it = remappings.find("__master");
|
||||
if (it != remappings.end())
|
||||
{
|
||||
g_uri = it->second;
|
||||
}
|
||||
|
||||
if (g_uri.empty())
|
||||
{
|
||||
char *master_uri_env = NULL;
|
||||
#ifdef _MSC_VER
|
||||
_dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
|
||||
#else
|
||||
master_uri_env = getenv("ROS_MASTER_URI");
|
||||
#endif
|
||||
if (!master_uri_env)
|
||||
{
|
||||
ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
|
||||
"type the following or (preferrably) add this to your " \
|
||||
"~/.bashrc file in order set up your " \
|
||||
"local machine as a ROS master:\n\n" \
|
||||
"export ROS_MASTER_URI=http://localhost:11311\n\n" \
|
||||
"then, type 'roscore' in another shell to actually launch " \
|
||||
"the master program.");
|
||||
ROS_BREAK();
|
||||
}
|
||||
|
||||
g_uri = master_uri_env;
|
||||
|
||||
#ifdef _MSC_VER
|
||||
// http://msdn.microsoft.com/en-us/library/ms175774(v=vs.80).aspx
|
||||
free(master_uri_env);
|
||||
#endif
|
||||
}
|
||||
|
||||
// Split URI into
|
||||
if (!network::splitURI(g_uri, g_host, g_port))
|
||||
{
|
||||
ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
|
||||
ROS_BREAK();
|
||||
}
|
||||
}
|
||||
|
||||
const std::string& getHost()
|
||||
{
|
||||
return g_host;
|
||||
}
|
||||
|
||||
uint32_t getPort()
|
||||
{
|
||||
return g_port;
|
||||
}
|
||||
|
||||
const std::string& getURI()
|
||||
{
|
||||
return g_uri;
|
||||
}
|
||||
|
||||
void setRetryTimeout(ros::WallDuration timeout)
|
||||
{
|
||||
if (timeout < ros::WallDuration(0))
|
||||
{
|
||||
ROS_FATAL("retry timeout must not be negative.");
|
||||
ROS_BREAK();
|
||||
}
|
||||
g_retry_timeout = timeout;
|
||||
}
|
||||
|
||||
bool check()
|
||||
{
|
||||
XmlRpc::XmlRpcValue args, result, payload;
|
||||
args[0] = this_node::getName();
|
||||
return execute("getPid", args, result, payload, false);
|
||||
}
|
||||
|
||||
bool getTopics(V_TopicInfo& topics)
|
||||
{
|
||||
XmlRpc::XmlRpcValue args, result, payload;
|
||||
args[0] = this_node::getName();
|
||||
args[1] = ""; //TODO: Fix this
|
||||
|
||||
if (!execute("getPublishedTopics", args, result, payload, true))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
topics.clear();
|
||||
for (int i = 0; i < payload.size(); i++)
|
||||
{
|
||||
topics.push_back(TopicInfo(std::string(payload[i][0]), std::string(payload[i][1])));
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool getNodes(V_string& nodes)
|
||||
{
|
||||
XmlRpc::XmlRpcValue args, result, payload;
|
||||
args[0] = this_node::getName();
|
||||
|
||||
if (!execute("getSystemState", args, result, payload, true))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
S_string node_set;
|
||||
for (int i = 0; i < payload.size(); ++i)
|
||||
{
|
||||
for (int j = 0; j < payload[i].size(); ++j)
|
||||
{
|
||||
XmlRpc::XmlRpcValue val = payload[i][j][1];
|
||||
for (int k = 0; k < val.size(); ++k)
|
||||
{
|
||||
std::string name = payload[i][j][1][k];
|
||||
node_set.insert(name);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
nodes.insert(nodes.end(), node_set.begin(), node_set.end());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
#if defined(__APPLE__)
|
||||
boost::mutex g_xmlrpc_call_mutex;
|
||||
#endif
|
||||
|
||||
bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master)
|
||||
{
|
||||
ros::SteadyTime start_time = ros::SteadyTime::now();
|
||||
|
||||
std::string master_host = getHost();
|
||||
uint32_t master_port = getPort();
|
||||
XmlRpc::XmlRpcClient *c = XMLRPCManager::instance()->getXMLRPCClient(master_host, master_port, "/");
|
||||
bool printed = false;
|
||||
bool slept = false;
|
||||
bool ok = true;
|
||||
bool b = false;
|
||||
do
|
||||
{
|
||||
{
|
||||
#if defined(__APPLE__)
|
||||
boost::mutex::scoped_lock lock(g_xmlrpc_call_mutex);
|
||||
#endif
|
||||
|
||||
b = c->execute(method.c_str(), request, response);
|
||||
}
|
||||
|
||||
ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
|
||||
|
||||
if (!b && ok)
|
||||
{
|
||||
if (!printed && wait_for_master)
|
||||
{
|
||||
ROS_ERROR("[%s] Failed to contact master at [%s:%d]. %s", method.c_str(), master_host.c_str(), master_port, wait_for_master ? "Retrying..." : "");
|
||||
printed = true;
|
||||
}
|
||||
|
||||
if (!wait_for_master)
|
||||
{
|
||||
XMLRPCManager::instance()->releaseXMLRPCClient(c);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!g_retry_timeout.isZero() && (ros::SteadyTime::now() - start_time) >= g_retry_timeout)
|
||||
{
|
||||
ROS_ERROR("[%s] Timed out trying to connect to the master after [%f] seconds", method.c_str(), g_retry_timeout.toSec());
|
||||
XMLRPCManager::instance()->releaseXMLRPCClient(c);
|
||||
return false;
|
||||
}
|
||||
|
||||
ros::WallDuration(0.05).sleep();
|
||||
slept = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!XMLRPCManager::instance()->validateXmlrpcResponse(method, response, payload))
|
||||
{
|
||||
XMLRPCManager::instance()->releaseXMLRPCClient(c);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
|
||||
} while(ok);
|
||||
|
||||
if (ok && slept)
|
||||
{
|
||||
ROS_INFO("Connected to master at [%s:%d]", master_host.c_str(), master_port);
|
||||
}
|
||||
|
||||
XMLRPCManager::instance()->releaseXMLRPCClient(c);
|
||||
|
||||
return b;
|
||||
}
|
||||
|
||||
} // namespace master
|
||||
|
||||
} // namespace ros
|
||||
86
thirdparty/ros/ros_comm/clients/roscpp/src/libros/message_deserializer.cpp
vendored
Normal file
86
thirdparty/ros/ros_comm/clients/roscpp/src/libros/message_deserializer.cpp
vendored
Normal file
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
|
||||
#include "ros/message_deserializer.h"
|
||||
#include "ros/subscription_callback_helper.h"
|
||||
#include <ros/console.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
MessageDeserializer::MessageDeserializer(const SubscriptionCallbackHelperPtr& helper, const SerializedMessage& m, const boost::shared_ptr<M_string>& connection_header)
|
||||
: helper_(helper)
|
||||
, serialized_message_(m)
|
||||
, connection_header_(connection_header)
|
||||
{
|
||||
if (serialized_message_.message && *serialized_message_.type_info != helper->getTypeInfo())
|
||||
{
|
||||
serialized_message_.message.reset();
|
||||
}
|
||||
}
|
||||
|
||||
VoidConstPtr MessageDeserializer::deserialize()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if (msg_)
|
||||
{
|
||||
return msg_;
|
||||
}
|
||||
|
||||
if (serialized_message_.message)
|
||||
{
|
||||
msg_ = serialized_message_.message;
|
||||
return msg_;
|
||||
}
|
||||
|
||||
if (!serialized_message_.buf && serialized_message_.num_bytes > 0)
|
||||
{
|
||||
// If the buffer has been reset it means we tried to deserialize and failed
|
||||
return VoidConstPtr();
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
SubscriptionCallbackHelperDeserializeParams params;
|
||||
params.buffer = serialized_message_.message_start;
|
||||
params.length = serialized_message_.num_bytes - (serialized_message_.message_start - serialized_message_.buf.get());
|
||||
params.connection_header = connection_header_;
|
||||
msg_ = helper_->deserialize(params);
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
ROS_ERROR("Exception thrown when deserializing message of length [%d] from [%s]: %s", (uint32_t)serialized_message_.num_bytes, (*connection_header_)["callerid"].c_str(), e.what());
|
||||
}
|
||||
|
||||
serialized_message_.buf.reset();
|
||||
|
||||
return msg_;
|
||||
}
|
||||
|
||||
}
|
||||
238
thirdparty/ros/ros_comm/clients/roscpp/src/libros/names.cpp
vendored
Normal file
238
thirdparty/ros/ros_comm/clients/roscpp/src/libros/names.cpp
vendored
Normal file
@@ -0,0 +1,238 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include "ros/names.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#include <ros/console.h>
|
||||
#include <ros/assert.h>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
namespace names
|
||||
{
|
||||
|
||||
M_string g_remappings;
|
||||
M_string g_unresolved_remappings;
|
||||
|
||||
const M_string& getRemappings()
|
||||
{
|
||||
return g_remappings;
|
||||
}
|
||||
|
||||
const M_string& getUnresolvedRemappings()
|
||||
{
|
||||
return g_unresolved_remappings;
|
||||
}
|
||||
|
||||
bool isValidCharInName(char c)
|
||||
{
|
||||
if (isalnum(c) || c == '/' || c == '_')
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool validate(const std::string& name, std::string& error)
|
||||
{
|
||||
if (name.empty())
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// First element is special, can be only ~ / or alpha
|
||||
char c = name[0];
|
||||
if (!isalpha(c) && c != '/' && c != '~')
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "Character [" << c << "] is not valid as the first character in Graph Resource Name [" << name << "]. Valid characters are a-z, A-Z, / and in some cases ~.";
|
||||
error = ss.str();
|
||||
return false;
|
||||
}
|
||||
|
||||
for (size_t i = 1; i < name.size(); ++i)
|
||||
{
|
||||
c = name[i];
|
||||
if (!isValidCharInName(c))
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "Character [" << c << "] at element [" << i << "] is not valid in Graph Resource Name [" << name <<"]. Valid characters are a-z, A-Z, 0-9, / and _.";
|
||||
error = ss.str();
|
||||
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
std::string clean(const std::string& name)
|
||||
{
|
||||
std::string clean = name;
|
||||
|
||||
size_t pos = clean.find("//");
|
||||
while (pos != std::string::npos)
|
||||
{
|
||||
clean.erase(pos, 1);
|
||||
pos = clean.find("//", pos);
|
||||
}
|
||||
|
||||
if (*clean.rbegin() == '/')
|
||||
{
|
||||
clean.erase(clean.size() - 1, 1);
|
||||
}
|
||||
|
||||
return clean;
|
||||
}
|
||||
|
||||
std::string append(const std::string& left, const std::string& right)
|
||||
{
|
||||
return clean(left + "/" + right);
|
||||
}
|
||||
|
||||
std::string remap(const std::string& name)
|
||||
{
|
||||
std::string resolved = resolve(name, false);
|
||||
|
||||
M_string::const_iterator it = g_remappings.find(resolved);
|
||||
if (it != g_remappings.end())
|
||||
{
|
||||
return it->second;
|
||||
}
|
||||
|
||||
return name;
|
||||
}
|
||||
|
||||
std::string resolve(const std::string& name, bool _remap)
|
||||
{
|
||||
std::string s = resolve(this_node::getNamespace(), name, _remap);
|
||||
return s;
|
||||
}
|
||||
|
||||
std::string resolve(const std::string& ns, const std::string& name, bool _remap)
|
||||
{
|
||||
std::string error;
|
||||
if (!validate(name, error))
|
||||
{
|
||||
throw InvalidNameException(error);
|
||||
}
|
||||
|
||||
if (name.empty())
|
||||
{
|
||||
if (ns.empty())
|
||||
{
|
||||
return "/";
|
||||
}
|
||||
|
||||
if (ns[0] == '/')
|
||||
{
|
||||
return ns;
|
||||
}
|
||||
|
||||
return append("/", ns);
|
||||
}
|
||||
|
||||
std::string copy = name;
|
||||
|
||||
if (copy[0] == '~')
|
||||
{
|
||||
copy = append(this_node::getName(), copy.substr(1));
|
||||
}
|
||||
|
||||
if (copy[0] != '/')
|
||||
{
|
||||
copy = append("/", append(ns, copy));
|
||||
}
|
||||
|
||||
copy = clean(copy);
|
||||
|
||||
if (_remap)
|
||||
{
|
||||
copy = remap(copy);
|
||||
}
|
||||
|
||||
return copy;
|
||||
}
|
||||
|
||||
void init(const M_string& remappings)
|
||||
{
|
||||
M_string::const_iterator it = remappings.begin();
|
||||
M_string::const_iterator end = remappings.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const std::string& left = it->first;
|
||||
const std::string& right = it->second;
|
||||
|
||||
if (!left.empty() && left[0] != '_' && left != this_node::getName())
|
||||
{
|
||||
std::string resolved_left = resolve(left, false);
|
||||
std::string resolved_right = resolve(right, false);
|
||||
g_remappings[resolved_left] = resolved_right;
|
||||
g_unresolved_remappings[left] = right;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::string parentNamespace(const std::string& name)
|
||||
{
|
||||
std::string error;
|
||||
if (!validate(name, error))
|
||||
{
|
||||
throw InvalidNameException(error);
|
||||
}
|
||||
|
||||
if (!name.compare("")) return "";
|
||||
if (!name.compare("/")) return "/";
|
||||
|
||||
std::string stripped_name;
|
||||
|
||||
// rstrip trailing slash
|
||||
if (name.find_last_of('/') == name.size()-1)
|
||||
stripped_name = name.substr(0, name.size() -2);
|
||||
else
|
||||
stripped_name = name;
|
||||
|
||||
//pull everything up to the last /
|
||||
size_t last_pos = stripped_name.find_last_of('/');
|
||||
if (last_pos == std::string::npos)
|
||||
{
|
||||
return "";
|
||||
}
|
||||
else if (last_pos == 0)
|
||||
return "/";
|
||||
return stripped_name.substr(0, last_pos);
|
||||
}
|
||||
|
||||
} // namespace names
|
||||
|
||||
} // namespace ros
|
||||
223
thirdparty/ros/ros_comm/clients/roscpp/src/libros/network.cpp
vendored
Normal file
223
thirdparty/ros/ros_comm/clients/roscpp/src/libros/network.cpp
vendored
Normal file
@@ -0,0 +1,223 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include "config.h"
|
||||
#include "ros/network.h"
|
||||
#include "ros/file_log.h"
|
||||
#include "ros/exceptions.h"
|
||||
#include "ros/io.h" // cross-platform headers needed
|
||||
#include <ros/console.h>
|
||||
#include <ros/assert.h>
|
||||
#ifdef HAVE_IFADDRS_H
|
||||
#include <ifaddrs.h>
|
||||
#endif
|
||||
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
namespace network
|
||||
{
|
||||
|
||||
std::string g_host;
|
||||
uint16_t g_tcpros_server_port = 0;
|
||||
|
||||
const std::string& getHost()
|
||||
{
|
||||
return g_host;
|
||||
}
|
||||
|
||||
bool splitURI(const std::string& uri, std::string& host, uint32_t& port)
|
||||
{
|
||||
// skip over the protocol if it's there
|
||||
if (uri.substr(0, 7) == std::string("http://"))
|
||||
host = uri.substr(7);
|
||||
else if (uri.substr(0, 9) == std::string("rosrpc://"))
|
||||
host = uri.substr(9);
|
||||
// split out the port
|
||||
std::string::size_type colon_pos = host.find_first_of(":");
|
||||
if (colon_pos == std::string::npos)
|
||||
return false;
|
||||
std::string port_str = host.substr(colon_pos+1);
|
||||
std::string::size_type slash_pos = port_str.find_first_of("/");
|
||||
if (slash_pos != std::string::npos)
|
||||
port_str = port_str.erase(slash_pos);
|
||||
port = atoi(port_str.c_str());
|
||||
host = host.erase(colon_pos);
|
||||
return true;
|
||||
}
|
||||
|
||||
uint16_t getTCPROSPort()
|
||||
{
|
||||
return g_tcpros_server_port;
|
||||
}
|
||||
|
||||
static bool isPrivateIP(const char *ip)
|
||||
{
|
||||
bool b = !strncmp("192.168", ip, 7) || !strncmp("10.", ip, 3) ||
|
||||
!strncmp("169.254", ip, 7);
|
||||
return b;
|
||||
}
|
||||
|
||||
std::string determineHost()
|
||||
{
|
||||
std::string ip_env;
|
||||
// First, did the user set ROS_HOSTNAME?
|
||||
if ( get_environment_variable(ip_env, "ROS_HOSTNAME")) {
|
||||
ROSCPP_LOG_DEBUG( "determineIP: using value of ROS_HOSTNAME:%s:", ip_env.c_str());
|
||||
if (ip_env.size() == 0)
|
||||
{
|
||||
ROS_WARN("invalid ROS_HOSTNAME (an empty string)");
|
||||
}
|
||||
return ip_env;
|
||||
}
|
||||
|
||||
// Second, did the user set ROS_IP?
|
||||
if ( get_environment_variable(ip_env, "ROS_IP")) {
|
||||
ROSCPP_LOG_DEBUG( "determineIP: using value of ROS_IP:%s:", ip_env.c_str());
|
||||
if (ip_env.size() == 0)
|
||||
{
|
||||
ROS_WARN("invalid ROS_IP (an empty string)");
|
||||
}
|
||||
return ip_env;
|
||||
}
|
||||
|
||||
// Third, try the hostname
|
||||
char host[1024];
|
||||
memset(host,0,sizeof(host));
|
||||
if(gethostname(host,sizeof(host)-1) != 0)
|
||||
{
|
||||
ROS_ERROR("determineIP: gethostname failed");
|
||||
}
|
||||
// We don't want localhost to be our ip
|
||||
else if(strlen(host) && strcmp("localhost", host))
|
||||
{
|
||||
return std::string(host);
|
||||
}
|
||||
|
||||
// Fourth, fall back on interface search, which will yield an IP address
|
||||
|
||||
#ifdef HAVE_IFADDRS_H
|
||||
struct ifaddrs *ifa = NULL, *ifp = NULL;
|
||||
int rc;
|
||||
if ((rc = getifaddrs(&ifp)) < 0)
|
||||
{
|
||||
ROS_FATAL("error in getifaddrs: [%s]", strerror(rc));
|
||||
ROS_BREAK();
|
||||
}
|
||||
char preferred_ip[200] = {0};
|
||||
for (ifa = ifp; ifa; ifa = ifa->ifa_next)
|
||||
{
|
||||
char ip_[200];
|
||||
socklen_t salen;
|
||||
if (!ifa->ifa_addr)
|
||||
continue; // evidently this interface has no ip address
|
||||
if (ifa->ifa_addr->sa_family == AF_INET)
|
||||
salen = sizeof(struct sockaddr_in);
|
||||
else if (ifa->ifa_addr->sa_family == AF_INET6)
|
||||
salen = sizeof(struct sockaddr_in6);
|
||||
else
|
||||
continue;
|
||||
if (getnameinfo(ifa->ifa_addr, salen, ip_, sizeof(ip_), NULL, 0,
|
||||
NI_NUMERICHOST) < 0)
|
||||
{
|
||||
ROSCPP_LOG_DEBUG( "getnameinfo couldn't get the ip of interface [%s]", ifa->ifa_name);
|
||||
continue;
|
||||
}
|
||||
//ROS_INFO( "ip of interface [%s] is [%s]", ifa->ifa_name, ip);
|
||||
// prefer non-private IPs over private IPs
|
||||
if (!strcmp("127.0.0.1", ip_) || strchr(ip_,':'))
|
||||
continue; // ignore loopback unless we have no other choice
|
||||
if (ifa->ifa_addr->sa_family == AF_INET6 && !preferred_ip[0])
|
||||
strcpy(preferred_ip, ip_);
|
||||
else if (isPrivateIP(ip_) && !preferred_ip[0])
|
||||
strcpy(preferred_ip, ip_);
|
||||
else if (!isPrivateIP(ip_) &&
|
||||
(isPrivateIP(preferred_ip) || !preferred_ip[0]))
|
||||
strcpy(preferred_ip, ip_);
|
||||
}
|
||||
freeifaddrs(ifp);
|
||||
if (!preferred_ip[0])
|
||||
{
|
||||
ROS_ERROR( "Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP "
|
||||
"address is 127.0.0.1. This should work for local processes, "
|
||||
"but will almost certainly not work if you have remote processes."
|
||||
"Report to the ROS development team to seek a fix.");
|
||||
return std::string("127.0.0.1");
|
||||
}
|
||||
ROSCPP_LOG_DEBUG( "preferred IP is guessed to be %s", preferred_ip);
|
||||
return std::string(preferred_ip);
|
||||
#else
|
||||
// @todo Fix IP determination in the case where getifaddrs() isn't
|
||||
// available.
|
||||
ROS_ERROR( "You don't have the getifaddrs() call; I'm assuming that your IP "
|
||||
"address is 127.0.0.1. This should work for local processes, "
|
||||
"but will almost certainly not work if you have remote processes."
|
||||
"Report to the ROS development team to seek a fix.");
|
||||
return std::string("127.0.0.1");
|
||||
#endif
|
||||
}
|
||||
|
||||
void init(const M_string& remappings)
|
||||
{
|
||||
M_string::const_iterator it = remappings.find("__hostname");
|
||||
if (it != remappings.end())
|
||||
{
|
||||
g_host = it->second;
|
||||
}
|
||||
else
|
||||
{
|
||||
it = remappings.find("__ip");
|
||||
if (it != remappings.end())
|
||||
{
|
||||
g_host = it->second;
|
||||
}
|
||||
}
|
||||
|
||||
it = remappings.find("__tcpros_server_port");
|
||||
if (it != remappings.end())
|
||||
{
|
||||
try
|
||||
{
|
||||
g_tcpros_server_port = boost::lexical_cast<uint16_t>(it->second);
|
||||
}
|
||||
catch (boost::bad_lexical_cast&)
|
||||
{
|
||||
throw ros::InvalidPortException("__tcpros_server_port [" + it->second + "] was not specified as a number within the 0-65535 range");
|
||||
}
|
||||
}
|
||||
|
||||
if (g_host.empty())
|
||||
{
|
||||
g_host = determineHost();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace network
|
||||
|
||||
} // namespace ros
|
||||
799
thirdparty/ros/ros_comm/clients/roscpp/src/libros/node_handle.cpp
vendored
Normal file
799
thirdparty/ros/ros_comm/clients/roscpp/src/libros/node_handle.cpp
vendored
Normal file
@@ -0,0 +1,799 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include "ros/node_handle.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/service.h"
|
||||
#include "ros/callback_queue.h"
|
||||
|
||||
#include "ros/time.h"
|
||||
#include "ros/rate.h"
|
||||
#include "ros/timer.h"
|
||||
#include "ros/wall_timer.h"
|
||||
#include "ros/steady_timer.h"
|
||||
|
||||
#include "ros/xmlrpc_manager.h"
|
||||
#include "ros/topic_manager.h"
|
||||
#include "ros/service_manager.h"
|
||||
#include "ros/master.h"
|
||||
#include "ros/param.h"
|
||||
#include "ros/names.h"
|
||||
#include "ros/init.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "xmlrpcpp/XmlRpc.h"
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
boost::mutex g_nh_refcount_mutex;
|
||||
int32_t g_nh_refcount = 0;
|
||||
bool g_node_started_by_nh = false;
|
||||
|
||||
class NodeHandleBackingCollection
|
||||
{
|
||||
public:
|
||||
typedef std::vector<Publisher::ImplWPtr> V_PubImpl;
|
||||
typedef std::vector<ServiceServer::ImplWPtr> V_SrvImpl;
|
||||
typedef std::vector<Subscriber::ImplWPtr> V_SubImpl;
|
||||
typedef std::vector<ServiceClient::ImplWPtr> V_SrvCImpl;
|
||||
V_PubImpl pubs_;
|
||||
V_SrvImpl srvs_;
|
||||
V_SubImpl subs_;
|
||||
V_SrvCImpl srv_cs_;
|
||||
|
||||
boost::mutex mutex_;
|
||||
};
|
||||
|
||||
NodeHandle::NodeHandle(const std::string& ns, const M_string& remappings)
|
||||
: namespace_(this_node::getNamespace())
|
||||
, callback_queue_(0)
|
||||
, collection_(0)
|
||||
{
|
||||
std::string tilde_resolved_ns;
|
||||
if (!ns.empty() && ns[0] == '~')// starts with tilde
|
||||
tilde_resolved_ns = names::resolve(ns);
|
||||
else
|
||||
tilde_resolved_ns = ns;
|
||||
|
||||
construct(tilde_resolved_ns, true);
|
||||
|
||||
initRemappings(remappings);
|
||||
}
|
||||
|
||||
NodeHandle::NodeHandle(const NodeHandle& parent, const std::string& ns)
|
||||
: collection_(0)
|
||||
{
|
||||
namespace_ = parent.getNamespace();
|
||||
callback_queue_ = parent.callback_queue_;
|
||||
|
||||
remappings_ = parent.remappings_;
|
||||
unresolved_remappings_ = parent.unresolved_remappings_;
|
||||
|
||||
construct(ns, false);
|
||||
}
|
||||
|
||||
NodeHandle::NodeHandle(const NodeHandle& parent, const std::string& ns, const M_string& remappings)
|
||||
: collection_(0)
|
||||
{
|
||||
namespace_ = parent.getNamespace();
|
||||
callback_queue_ = parent.callback_queue_;
|
||||
|
||||
remappings_ = parent.remappings_;
|
||||
unresolved_remappings_ = parent.unresolved_remappings_;
|
||||
|
||||
construct(ns, false);
|
||||
|
||||
initRemappings(remappings);
|
||||
}
|
||||
|
||||
NodeHandle::NodeHandle(const NodeHandle& rhs)
|
||||
: collection_(0)
|
||||
{
|
||||
callback_queue_ = rhs.callback_queue_;
|
||||
remappings_ = rhs.remappings_;
|
||||
unresolved_remappings_ = rhs.unresolved_remappings_;
|
||||
|
||||
construct(rhs.namespace_, true);
|
||||
|
||||
unresolved_namespace_ = rhs.unresolved_namespace_;
|
||||
}
|
||||
|
||||
NodeHandle::~NodeHandle()
|
||||
{
|
||||
destruct();
|
||||
}
|
||||
|
||||
NodeHandle& NodeHandle::operator=(const NodeHandle& rhs)
|
||||
{
|
||||
ROS_ASSERT(collection_);
|
||||
namespace_ = rhs.namespace_;
|
||||
callback_queue_ = rhs.callback_queue_;
|
||||
remappings_ = rhs.remappings_;
|
||||
unresolved_remappings_ = rhs.unresolved_remappings_;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
void spinThread()
|
||||
{
|
||||
ros::spin();
|
||||
}
|
||||
|
||||
void NodeHandle::construct(const std::string& ns, bool validate_name)
|
||||
{
|
||||
if (!ros::isInitialized())
|
||||
{
|
||||
ROS_FATAL("You must call ros::init() before creating the first NodeHandle");
|
||||
ROS_BREAK();
|
||||
}
|
||||
|
||||
collection_ = new NodeHandleBackingCollection;
|
||||
unresolved_namespace_ = ns;
|
||||
// if callback_queue_ is nonnull, we are in a non-nullary constructor
|
||||
|
||||
if (validate_name)
|
||||
namespace_ = resolveName(ns, true);
|
||||
else
|
||||
{
|
||||
namespace_ = resolveName(ns, true, no_validate());
|
||||
// FIXME validate namespace_ now
|
||||
}
|
||||
ok_ = true;
|
||||
|
||||
boost::mutex::scoped_lock lock(g_nh_refcount_mutex);
|
||||
|
||||
if (g_nh_refcount == 0 && !ros::isStarted())
|
||||
{
|
||||
g_node_started_by_nh = true;
|
||||
ros::start();
|
||||
}
|
||||
|
||||
++g_nh_refcount;
|
||||
}
|
||||
|
||||
void NodeHandle::destruct()
|
||||
{
|
||||
delete collection_;
|
||||
|
||||
boost::mutex::scoped_lock lock(g_nh_refcount_mutex);
|
||||
|
||||
--g_nh_refcount;
|
||||
|
||||
if (g_nh_refcount == 0 && g_node_started_by_nh)
|
||||
{
|
||||
ros::shutdown();
|
||||
}
|
||||
}
|
||||
|
||||
void NodeHandle::initRemappings(const M_string& remappings)
|
||||
{
|
||||
{
|
||||
M_string::const_iterator it = remappings.begin();
|
||||
M_string::const_iterator end = remappings.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const std::string& from = it->first;
|
||||
const std::string& to = it->second;
|
||||
|
||||
remappings_.insert(std::make_pair(resolveName(from, false), resolveName(to, false)));
|
||||
unresolved_remappings_.insert(std::make_pair(from, to));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NodeHandle::setCallbackQueue(CallbackQueueInterface* queue)
|
||||
{
|
||||
callback_queue_ = queue;
|
||||
}
|
||||
|
||||
std::string NodeHandle::remapName(const std::string& name) const
|
||||
{
|
||||
std::string resolved = resolveName(name, false);
|
||||
|
||||
// First search any remappings that were passed in specifically for this NodeHandle
|
||||
M_string::const_iterator it = remappings_.find(resolved);
|
||||
if (it != remappings_.end())
|
||||
{
|
||||
// ROSCPP_LOG_DEBUG("found 'local' remapping: %s", it->second.c_str());
|
||||
return it->second;
|
||||
}
|
||||
|
||||
// If not in our local remappings, perhaps in the global ones
|
||||
return names::remap(resolved);
|
||||
}
|
||||
|
||||
std::string NodeHandle::resolveName(const std::string& name, bool remap) const
|
||||
{
|
||||
// ROSCPP_LOG_DEBUG("resolveName(%s, %s)", name.c_str(), remap ? "true" : "false");
|
||||
std::string error;
|
||||
if (!names::validate(name, error))
|
||||
{
|
||||
throw InvalidNameException(error);
|
||||
}
|
||||
|
||||
return resolveName(name, remap, no_validate());
|
||||
}
|
||||
|
||||
std::string NodeHandle::resolveName(const std::string& name, bool remap, no_validate) const
|
||||
{
|
||||
if (name.empty())
|
||||
{
|
||||
return namespace_;
|
||||
}
|
||||
|
||||
std::string final = name;
|
||||
|
||||
if (final[0] == '~')
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "Using ~ names with NodeHandle methods is not allowed. If you want to use private names with the NodeHandle ";
|
||||
ss << "interface, construct a NodeHandle using a private name as its namespace. e.g. ";
|
||||
ss << "ros::NodeHandle nh(\"~\"); ";
|
||||
ss << "nh.getParam(\"my_private_name\");";
|
||||
ss << " (name = [" << name << "])";
|
||||
throw InvalidNameException(ss.str());
|
||||
}
|
||||
else if (final[0] == '/')
|
||||
{
|
||||
// do nothing
|
||||
}
|
||||
else if (!namespace_.empty())
|
||||
{
|
||||
// ROSCPP_LOG_DEBUG("Appending namespace_ (%s)", namespace_.c_str());
|
||||
final = names::append(namespace_, final);
|
||||
}
|
||||
|
||||
// ROSCPP_LOG_DEBUG("resolveName, pre-clean: %s", final.c_str());
|
||||
final = names::clean(final);
|
||||
// ROSCPP_LOG_DEBUG("resolveName, post-clean: %s", final.c_str());
|
||||
|
||||
if (remap)
|
||||
{
|
||||
final = remapName(final);
|
||||
// ROSCPP_LOG_DEBUG("resolveName, remapped: %s", final.c_str());
|
||||
}
|
||||
|
||||
return names::resolve(final, false);
|
||||
}
|
||||
|
||||
Publisher NodeHandle::advertise(AdvertiseOptions& ops)
|
||||
{
|
||||
ops.topic = resolveName(ops.topic);
|
||||
if (ops.callback_queue == 0)
|
||||
{
|
||||
if (callback_queue_)
|
||||
{
|
||||
ops.callback_queue = callback_queue_;
|
||||
}
|
||||
else
|
||||
{
|
||||
ops.callback_queue = getGlobalCallbackQueue();
|
||||
}
|
||||
}
|
||||
|
||||
SubscriberCallbacksPtr callbacks(boost::make_shared<SubscriberCallbacks>(ops.connect_cb, ops.disconnect_cb,
|
||||
ops.tracked_object, ops.callback_queue));
|
||||
|
||||
if (TopicManager::instance()->advertise(ops, callbacks))
|
||||
{
|
||||
Publisher pub(ops.topic, ops.md5sum, ops.datatype, *this, callbacks);
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(collection_->mutex_);
|
||||
collection_->pubs_.push_back(pub.impl_);
|
||||
}
|
||||
|
||||
return pub;
|
||||
}
|
||||
|
||||
return Publisher();
|
||||
}
|
||||
|
||||
Subscriber NodeHandle::subscribe(SubscribeOptions& ops)
|
||||
{
|
||||
ops.topic = resolveName(ops.topic);
|
||||
if (ops.callback_queue == 0)
|
||||
{
|
||||
if (callback_queue_)
|
||||
{
|
||||
ops.callback_queue = callback_queue_;
|
||||
}
|
||||
else
|
||||
{
|
||||
ops.callback_queue = getGlobalCallbackQueue();
|
||||
}
|
||||
}
|
||||
|
||||
if (TopicManager::instance()->subscribe(ops))
|
||||
{
|
||||
Subscriber sub(ops.topic, *this, ops.helper);
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(collection_->mutex_);
|
||||
collection_->subs_.push_back(sub.impl_);
|
||||
}
|
||||
|
||||
return sub;
|
||||
}
|
||||
|
||||
return Subscriber();
|
||||
}
|
||||
|
||||
ServiceServer NodeHandle::advertiseService(AdvertiseServiceOptions& ops)
|
||||
{
|
||||
ops.service = resolveName(ops.service);
|
||||
if (ops.callback_queue == 0)
|
||||
{
|
||||
if (callback_queue_)
|
||||
{
|
||||
ops.callback_queue = callback_queue_;
|
||||
}
|
||||
else
|
||||
{
|
||||
ops.callback_queue = getGlobalCallbackQueue();
|
||||
}
|
||||
}
|
||||
|
||||
if (ServiceManager::instance()->advertiseService(ops))
|
||||
{
|
||||
ServiceServer srv(ops.service, *this);
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(collection_->mutex_);
|
||||
collection_->srvs_.push_back(srv.impl_);
|
||||
}
|
||||
|
||||
return srv;
|
||||
}
|
||||
|
||||
return ServiceServer();
|
||||
}
|
||||
|
||||
ServiceClient NodeHandle::serviceClient(ServiceClientOptions& ops)
|
||||
{
|
||||
ops.service = resolveName(ops.service);
|
||||
ServiceClient client(ops.service, ops.persistent, ops.header, ops.md5sum);
|
||||
|
||||
if (client)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(collection_->mutex_);
|
||||
collection_->srv_cs_.push_back(client.impl_);
|
||||
}
|
||||
|
||||
return client;
|
||||
}
|
||||
|
||||
Timer NodeHandle::createTimer(Duration period, const TimerCallback& callback,
|
||||
bool oneshot, bool autostart) const
|
||||
{
|
||||
TimerOptions ops;
|
||||
ops.period = period;
|
||||
ops.callback = callback;
|
||||
ops.oneshot = oneshot;
|
||||
ops.autostart = autostart;
|
||||
return createTimer(ops);
|
||||
}
|
||||
|
||||
Timer NodeHandle::createTimer(TimerOptions& ops) const
|
||||
{
|
||||
if (ops.callback_queue == 0)
|
||||
{
|
||||
if (callback_queue_)
|
||||
{
|
||||
ops.callback_queue = callback_queue_;
|
||||
}
|
||||
else
|
||||
{
|
||||
ops.callback_queue = getGlobalCallbackQueue();
|
||||
}
|
||||
}
|
||||
|
||||
Timer timer(ops);
|
||||
if (ops.autostart)
|
||||
timer.start();
|
||||
return timer;
|
||||
}
|
||||
|
||||
WallTimer NodeHandle::createWallTimer(WallDuration period, const WallTimerCallback& callback,
|
||||
bool oneshot, bool autostart) const
|
||||
{
|
||||
WallTimerOptions ops;
|
||||
ops.period = period;
|
||||
ops.callback = callback;
|
||||
ops.oneshot = oneshot;
|
||||
ops.autostart = autostart;
|
||||
return createWallTimer(ops);
|
||||
}
|
||||
|
||||
WallTimer NodeHandle::createWallTimer(WallTimerOptions& ops) const
|
||||
{
|
||||
if (ops.callback_queue == 0)
|
||||
{
|
||||
if (callback_queue_)
|
||||
{
|
||||
ops.callback_queue = callback_queue_;
|
||||
}
|
||||
else
|
||||
{
|
||||
ops.callback_queue = getGlobalCallbackQueue();
|
||||
}
|
||||
}
|
||||
|
||||
WallTimer timer(ops);
|
||||
if (ops.autostart)
|
||||
timer.start();
|
||||
return timer;
|
||||
}
|
||||
|
||||
SteadyTimer NodeHandle::createSteadyTimer(WallDuration period, const SteadyTimerCallback& callback,
|
||||
bool oneshot, bool autostart) const
|
||||
{
|
||||
SteadyTimerOptions ops;
|
||||
ops.period = period;
|
||||
ops.callback = callback;
|
||||
ops.oneshot = oneshot;
|
||||
ops.autostart = autostart;
|
||||
return createSteadyTimer(ops);
|
||||
}
|
||||
|
||||
SteadyTimer NodeHandle::createSteadyTimer(SteadyTimerOptions& ops) const
|
||||
{
|
||||
if (ops.callback_queue == 0)
|
||||
{
|
||||
if (callback_queue_)
|
||||
{
|
||||
ops.callback_queue = callback_queue_;
|
||||
}
|
||||
else
|
||||
{
|
||||
ops.callback_queue = getGlobalCallbackQueue();
|
||||
}
|
||||
}
|
||||
|
||||
SteadyTimer timer(ops);
|
||||
if (ops.autostart)
|
||||
timer.start();
|
||||
return timer;
|
||||
}
|
||||
|
||||
void NodeHandle::shutdown()
|
||||
{
|
||||
{
|
||||
NodeHandleBackingCollection::V_SubImpl::iterator it = collection_->subs_.begin();
|
||||
NodeHandleBackingCollection::V_SubImpl::iterator end = collection_->subs_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
Subscriber::ImplPtr impl = it->lock();
|
||||
|
||||
if (impl)
|
||||
{
|
||||
impl->unsubscribe();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
NodeHandleBackingCollection::V_PubImpl::iterator it = collection_->pubs_.begin();
|
||||
NodeHandleBackingCollection::V_PubImpl::iterator end = collection_->pubs_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
Publisher::ImplPtr impl = it->lock();
|
||||
|
||||
if (impl)
|
||||
{
|
||||
impl->unadvertise();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
NodeHandleBackingCollection::V_SrvImpl::iterator it = collection_->srvs_.begin();
|
||||
NodeHandleBackingCollection::V_SrvImpl::iterator end = collection_->srvs_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
ServiceServer::ImplPtr impl = it->lock();
|
||||
|
||||
if (impl)
|
||||
{
|
||||
impl->unadvertise();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
NodeHandleBackingCollection::V_SrvCImpl::iterator it = collection_->srv_cs_.begin();
|
||||
NodeHandleBackingCollection::V_SrvCImpl::iterator end = collection_->srv_cs_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
ServiceClient::ImplPtr impl = it->lock();
|
||||
|
||||
if (impl)
|
||||
{
|
||||
impl->shutdown();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ok_ = false;
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, const XmlRpc::XmlRpcValue& v) const
|
||||
{
|
||||
return param::set(resolveName(key), v);
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, const std::string& s) const
|
||||
{
|
||||
return param::set(resolveName(key), s);
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, const char* s) const
|
||||
{
|
||||
return param::set(resolveName(key), s);
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, double d) const
|
||||
{
|
||||
return param::set(resolveName(key), d);
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, int i) const
|
||||
{
|
||||
return param::set(resolveName(key), i);
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, bool b) const
|
||||
{
|
||||
return param::set(resolveName(key), b);
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, const std::vector<std::string>& vec) const
|
||||
{
|
||||
return param::set(resolveName(key), vec);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::vector<double>& vec) const
|
||||
{
|
||||
return param::set(resolveName(key), vec);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::vector<float>& vec) const
|
||||
{
|
||||
return param::set(resolveName(key), vec);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::vector<int>& vec) const
|
||||
{
|
||||
return param::set(resolveName(key), vec);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::vector<bool>& vec) const
|
||||
{
|
||||
return param::set(resolveName(key), vec);
|
||||
}
|
||||
|
||||
void NodeHandle::setParam(const std::string& key, const std::map<std::string, std::string>& map) const
|
||||
{
|
||||
return param::set(resolveName(key), map);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::map<std::string, double>& map) const
|
||||
{
|
||||
return param::set(resolveName(key), map);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::map<std::string, float>& map) const
|
||||
{
|
||||
return param::set(resolveName(key), map);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::map<std::string, int>& map) const
|
||||
{
|
||||
return param::set(resolveName(key), map);
|
||||
}
|
||||
void NodeHandle::setParam(const std::string& key, const std::map<std::string, bool>& map) const
|
||||
{
|
||||
return param::set(resolveName(key), map);
|
||||
}
|
||||
|
||||
bool NodeHandle::hasParam(const std::string& key) const
|
||||
{
|
||||
return param::has(resolveName(key));
|
||||
}
|
||||
|
||||
bool NodeHandle::deleteParam(const std::string& key) const
|
||||
{
|
||||
return param::del(resolveName(key));
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamNames(std::vector<std::string>& keys) const
|
||||
{
|
||||
return param::getParamNames(keys);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, XmlRpc::XmlRpcValue& v) const
|
||||
{
|
||||
return param::get(resolveName(key), v);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, std::string& s) const
|
||||
{
|
||||
return param::get(resolveName(key), s);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, double& d) const
|
||||
{
|
||||
return param::get(resolveName(key), d);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, float& f) const
|
||||
{
|
||||
return param::get(resolveName(key), f);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, int& i) const
|
||||
{
|
||||
return param::get(resolveName(key), i);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, bool& b) const
|
||||
{
|
||||
return param::get(resolveName(key), b);
|
||||
}
|
||||
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, std::vector<std::string>& vec) const
|
||||
{
|
||||
return param::get(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::vector<double>& vec) const
|
||||
{
|
||||
return param::get(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::vector<float>& vec) const
|
||||
{
|
||||
return param::get(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::vector<int>& vec) const
|
||||
{
|
||||
return param::get(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::vector<bool>& vec) const
|
||||
{
|
||||
return param::get(resolveName(key), vec);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParam(const std::string& key, std::map<std::string, std::string>& map) const
|
||||
{
|
||||
return param::get(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::map<std::string, double>& map) const
|
||||
{
|
||||
return param::get(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::map<std::string, float>& map) const
|
||||
{
|
||||
return param::get(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::map<std::string, int>& map) const
|
||||
{
|
||||
return param::get(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParam(const std::string& key, std::map<std::string, bool>& map) const
|
||||
{
|
||||
return param::get(resolveName(key), map);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, XmlRpc::XmlRpcValue& v) const
|
||||
{
|
||||
return param::getCached(resolveName(key), v);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::string& s) const
|
||||
{
|
||||
return param::getCached(resolveName(key), s);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, double& d) const
|
||||
{
|
||||
return param::getCached(resolveName(key), d);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, float& f) const
|
||||
{
|
||||
return param::getCached(resolveName(key), f);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, int& i) const
|
||||
{
|
||||
return param::getCached(resolveName(key), i);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, bool& b) const
|
||||
{
|
||||
return param::getCached(resolveName(key), b);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::vector<std::string>& vec) const
|
||||
{
|
||||
return param::getCached(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::vector<double>& vec) const
|
||||
{
|
||||
return param::getCached(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::vector<float>& vec) const
|
||||
{
|
||||
return param::getCached(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::vector<int>& vec) const
|
||||
{
|
||||
return param::getCached(resolveName(key), vec);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::vector<bool>& vec) const
|
||||
{
|
||||
return param::getCached(resolveName(key), vec);
|
||||
}
|
||||
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, std::string>& map) const
|
||||
{
|
||||
return param::getCached(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, double>& map) const
|
||||
{
|
||||
return param::getCached(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, float>& map) const
|
||||
{
|
||||
return param::getCached(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, int>& map) const
|
||||
{
|
||||
return param::getCached(resolveName(key), map);
|
||||
}
|
||||
bool NodeHandle::getParamCached(const std::string& key, std::map<std::string, bool>& map) const
|
||||
{
|
||||
return param::getCached(resolveName(key), map);
|
||||
}
|
||||
|
||||
bool NodeHandle::searchParam(const std::string& key, std::string& result_out) const
|
||||
{
|
||||
// searchParam needs a separate form of remapping -- remapping on the unresolved name, rather than the
|
||||
// resolved one.
|
||||
|
||||
std::string remapped = key;
|
||||
M_string::const_iterator it = unresolved_remappings_.find(key);
|
||||
// First try our local remappings
|
||||
if (it != unresolved_remappings_.end())
|
||||
{
|
||||
remapped = it->second;
|
||||
}
|
||||
|
||||
return param::search(resolveName(""), remapped, result_out);
|
||||
}
|
||||
|
||||
bool NodeHandle::ok() const
|
||||
{
|
||||
return ros::ok() && ok_;
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
882
thirdparty/ros/ros_comm/clients/roscpp/src/libros/param.cpp
vendored
Normal file
882
thirdparty/ros/ros_comm/clients/roscpp/src/libros/param.cpp
vendored
Normal file
@@ -0,0 +1,882 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#include "ros/param.h"
|
||||
#include "ros/master.h"
|
||||
#include "ros/xmlrpc_manager.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/names.h"
|
||||
|
||||
#include <ros/console.h>
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
namespace param
|
||||
{
|
||||
|
||||
typedef std::map<std::string, XmlRpc::XmlRpcValue> M_Param;
|
||||
M_Param g_params;
|
||||
boost::mutex g_params_mutex;
|
||||
S_string g_subscribed_params;
|
||||
|
||||
void invalidateParentParams(const std::string& key)
|
||||
{
|
||||
std::string ns_key = names::parentNamespace(key);
|
||||
while (ns_key != "" && ns_key != "/")
|
||||
{
|
||||
if (g_subscribed_params.find(ns_key) != g_subscribed_params.end())
|
||||
{
|
||||
// by erasing the key the parameter will be re-queried
|
||||
g_params.erase(ns_key);
|
||||
}
|
||||
ns_key = names::parentNamespace(ns_key);
|
||||
}
|
||||
}
|
||||
|
||||
void set(const std::string& key, const XmlRpc::XmlRpcValue& v)
|
||||
{
|
||||
std::string mapped_key = ros::names::resolve(key);
|
||||
|
||||
XmlRpc::XmlRpcValue params, result, payload;
|
||||
params[0] = this_node::getName();
|
||||
params[1] = mapped_key;
|
||||
params[2] = v;
|
||||
|
||||
{
|
||||
// Lock around the execute to the master in case we get a parameter update on this value between
|
||||
// executing on the master and setting the parameter in the g_params list.
|
||||
boost::mutex::scoped_lock lock(g_params_mutex);
|
||||
|
||||
if (master::execute("setParam", params, result, payload, true))
|
||||
{
|
||||
// Update our cached params list now so that if get() is called immediately after param::set()
|
||||
// we already have the cached state and our value will be correct
|
||||
if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
|
||||
{
|
||||
g_params[mapped_key] = v;
|
||||
}
|
||||
invalidateParentParams(mapped_key);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::string& s)
|
||||
{
|
||||
// construct xmlrpc_c::value object of the std::string and
|
||||
// call param::set(key, xmlvalue);
|
||||
XmlRpc::XmlRpcValue v(s);
|
||||
ros::param::set(key, v);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const char* s)
|
||||
{
|
||||
// construct xmlrpc_c::value object of the std::string and
|
||||
// call param::set(key, xmlvalue);
|
||||
std::string sxx = std::string(s);
|
||||
XmlRpc::XmlRpcValue v(sxx);
|
||||
ros::param::set(key, v);
|
||||
}
|
||||
|
||||
void set(const std::string& key, double d)
|
||||
{
|
||||
XmlRpc::XmlRpcValue v(d);
|
||||
ros::param::set(key, v);
|
||||
}
|
||||
|
||||
void set(const std::string& key, int i)
|
||||
{
|
||||
XmlRpc::XmlRpcValue v(i);
|
||||
ros::param::set(key, v);
|
||||
}
|
||||
|
||||
void set(const std::string& key, bool b)
|
||||
{
|
||||
XmlRpc::XmlRpcValue v(b);
|
||||
ros::param::set(key, v);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
void setImpl(const std::string& key, const std::vector<T>& vec)
|
||||
{
|
||||
// Note: the XmlRpcValue starts off as "invalid" and assertArray turns it
|
||||
// into an array type with the given size
|
||||
XmlRpc::XmlRpcValue xml_vec;
|
||||
xml_vec.setSize(vec.size());
|
||||
|
||||
// Copy the contents into the XmlRpcValue
|
||||
for(size_t i=0; i < vec.size(); i++) {
|
||||
xml_vec[i] = vec.at(i);
|
||||
}
|
||||
|
||||
ros::param::set(key, xml_vec);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::vector<std::string>& vec)
|
||||
{
|
||||
setImpl(key, vec);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::vector<double>& vec)
|
||||
{
|
||||
setImpl(key, vec);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::vector<float>& vec)
|
||||
{
|
||||
setImpl(key, vec);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::vector<int>& vec)
|
||||
{
|
||||
setImpl(key, vec);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::vector<bool>& vec)
|
||||
{
|
||||
setImpl(key, vec);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
void setImpl(const std::string& key, const std::map<std::string, T>& map)
|
||||
{
|
||||
// Note: the XmlRpcValue starts off as "invalid" and assertStruct turns it
|
||||
// into a struct type
|
||||
XmlRpc::XmlRpcValue xml_value;
|
||||
xml_value.begin();
|
||||
|
||||
// Copy the contents into the XmlRpcValue
|
||||
for(typename std::map<std::string, T>::const_iterator it = map.begin(); it != map.end(); ++it) {
|
||||
xml_value[it->first] = it->second;
|
||||
}
|
||||
|
||||
ros::param::set(key, xml_value);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::map<std::string, std::string>& map)
|
||||
{
|
||||
setImpl(key, map);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::map<std::string, double>& map)
|
||||
{
|
||||
setImpl(key, map);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::map<std::string, float>& map)
|
||||
{
|
||||
setImpl(key, map);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::map<std::string, int>& map)
|
||||
{
|
||||
setImpl(key, map);
|
||||
}
|
||||
|
||||
void set(const std::string& key, const std::map<std::string, bool>& map)
|
||||
{
|
||||
setImpl(key, map);
|
||||
}
|
||||
|
||||
bool has(const std::string& key)
|
||||
{
|
||||
XmlRpc::XmlRpcValue params, result, payload;
|
||||
params[0] = this_node::getName();
|
||||
params[1] = ros::names::resolve(key);
|
||||
//params[1] = key;
|
||||
// We don't loop here, because validateXmlrpcResponse() returns false
|
||||
// both when we can't contact the master and when the master says, "I
|
||||
// don't have that param."
|
||||
if (!master::execute("hasParam", params, result, payload, false))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return payload;
|
||||
}
|
||||
|
||||
bool del(const std::string& key)
|
||||
{
|
||||
std::string mapped_key = ros::names::resolve(key);
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(g_params_mutex);
|
||||
|
||||
g_subscribed_params.erase(mapped_key);
|
||||
g_params.erase(mapped_key);
|
||||
}
|
||||
|
||||
XmlRpc::XmlRpcValue params, result, payload;
|
||||
params[0] = this_node::getName();
|
||||
params[1] = mapped_key;
|
||||
// We don't loop here, because validateXmlrpcResponse() returns false
|
||||
// both when we can't contact the master and when the master says, "I
|
||||
// don't have that param."
|
||||
if (!master::execute("deleteParam", params, result, payload, false))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool getImpl(const std::string& key, XmlRpc::XmlRpcValue& v, bool use_cache)
|
||||
{
|
||||
std::string mapped_key = ros::names::resolve(key);
|
||||
if (mapped_key.empty()) mapped_key = "/";
|
||||
|
||||
if (use_cache)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(g_params_mutex);
|
||||
|
||||
if (g_subscribed_params.find(mapped_key) != g_subscribed_params.end())
|
||||
{
|
||||
M_Param::iterator it = g_params.find(mapped_key);
|
||||
if (it != g_params.end())
|
||||
{
|
||||
if (it->second.valid())
|
||||
{
|
||||
ROS_DEBUG_NAMED("cached_parameters", "Using cached parameter value for key [%s]", mapped_key.c_str());
|
||||
|
||||
v = it->second;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_DEBUG_NAMED("cached_parameters", "Cached parameter is invalid for key [%s]", mapped_key.c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// parameter we've never seen before, register for update from the master
|
||||
if (g_subscribed_params.insert(mapped_key).second)
|
||||
{
|
||||
XmlRpc::XmlRpcValue params, result, payload;
|
||||
params[0] = this_node::getName();
|
||||
params[1] = XMLRPCManager::instance()->getServerURI();
|
||||
params[2] = mapped_key;
|
||||
|
||||
if (!master::execute("subscribeParam", params, result, payload, false))
|
||||
{
|
||||
ROS_DEBUG_NAMED("cached_parameters", "Subscribe to parameter [%s]: call to the master failed", mapped_key.c_str());
|
||||
g_subscribed_params.erase(mapped_key);
|
||||
use_cache = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_DEBUG_NAMED("cached_parameters", "Subscribed to parameter [%s]", mapped_key.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
XmlRpc::XmlRpcValue params, result;
|
||||
params[0] = this_node::getName();
|
||||
params[1] = mapped_key;
|
||||
|
||||
// We don't loop here, because validateXmlrpcResponse() returns false
|
||||
// both when we can't contact the master and when the master says, "I
|
||||
// don't have that param."
|
||||
bool ret = master::execute("getParam", params, result, v, false);
|
||||
|
||||
if (use_cache)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(g_params_mutex);
|
||||
|
||||
ROS_DEBUG_NAMED("cached_parameters", "Caching parameter [%s] with value type [%d]", mapped_key.c_str(), v.getType());
|
||||
g_params[mapped_key] = v;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool getImpl(const std::string& key, std::string& s, bool use_cache)
|
||||
{
|
||||
XmlRpc::XmlRpcValue v;
|
||||
if (!getImpl(key, v, use_cache))
|
||||
return false;
|
||||
if (v.getType() != XmlRpc::XmlRpcValue::TypeString)
|
||||
return false;
|
||||
s = std::string(v);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool getImpl(const std::string& key, double& d, bool use_cache)
|
||||
{
|
||||
XmlRpc::XmlRpcValue v;
|
||||
if (!getImpl(key, v, use_cache))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
|
||||
{
|
||||
d = (int)v;
|
||||
}
|
||||
else if (v.getType() != XmlRpc::XmlRpcValue::TypeDouble)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
d = v;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool getImpl(const std::string& key, float& f, bool use_cache)
|
||||
{
|
||||
double d = static_cast<double>(f);
|
||||
bool result = getImpl(key, d, use_cache);
|
||||
if (result)
|
||||
f = static_cast<float>(d);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool getImpl(const std::string& key, int& i, bool use_cache)
|
||||
{
|
||||
XmlRpc::XmlRpcValue v;
|
||||
if (!getImpl(key, v, use_cache))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble)
|
||||
{
|
||||
double d = v;
|
||||
|
||||
if (fmod(d, 1.0) < 0.5)
|
||||
{
|
||||
d = floor(d);
|
||||
}
|
||||
else
|
||||
{
|
||||
d = ceil(d);
|
||||
}
|
||||
|
||||
i = d;
|
||||
}
|
||||
else if (v.getType() != XmlRpc::XmlRpcValue::TypeInt)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
i = v;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool getImpl(const std::string& key, bool& b, bool use_cache)
|
||||
{
|
||||
XmlRpc::XmlRpcValue v;
|
||||
if (!getImpl(key, v, use_cache))
|
||||
return false;
|
||||
if (v.getType() != XmlRpc::XmlRpcValue::TypeBoolean)
|
||||
return false;
|
||||
b = v;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool get(const std::string& key, std::string& s)
|
||||
{
|
||||
return getImpl(key, s, false);
|
||||
}
|
||||
|
||||
bool get(const std::string& key, double& d)
|
||||
{
|
||||
return getImpl(key, d, false);
|
||||
}
|
||||
|
||||
bool get(const std::string& key, float& f)
|
||||
{
|
||||
return getImpl(key, f, false);
|
||||
}
|
||||
|
||||
bool get(const std::string& key, int& i)
|
||||
{
|
||||
return getImpl(key, i, false);
|
||||
}
|
||||
|
||||
bool get(const std::string& key, bool& b)
|
||||
{
|
||||
return getImpl(key, b, false);
|
||||
}
|
||||
|
||||
bool get(const std::string& key, XmlRpc::XmlRpcValue& v)
|
||||
{
|
||||
return getImpl(key, v, false);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, std::string& s)
|
||||
{
|
||||
return getImpl(key, s, true);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, double& d)
|
||||
{
|
||||
return getImpl(key, d, true);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, float& f)
|
||||
{
|
||||
return getImpl(key, f, true);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, int& i)
|
||||
{
|
||||
return getImpl(key, i, true);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, bool& b)
|
||||
{
|
||||
return getImpl(key, b, true);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, XmlRpc::XmlRpcValue& v)
|
||||
{
|
||||
return getImpl(key, v, true);
|
||||
}
|
||||
|
||||
template <class T> T xml_cast(XmlRpc::XmlRpcValue xml_value)
|
||||
{
|
||||
return static_cast<T>(xml_value);
|
||||
}
|
||||
|
||||
template <class T> bool xml_castable(int XmlType)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
template<> bool xml_castable<std::string>(int XmlType)
|
||||
{
|
||||
return XmlType == XmlRpc::XmlRpcValue::TypeString;
|
||||
}
|
||||
|
||||
template<> bool xml_castable<double>(int XmlType)
|
||||
{
|
||||
return (
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeInt ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
|
||||
}
|
||||
|
||||
template<> bool xml_castable<float>(int XmlType)
|
||||
{
|
||||
return (
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeInt ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
|
||||
}
|
||||
|
||||
template<> bool xml_castable<int>(int XmlType)
|
||||
{
|
||||
return (
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeInt ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
|
||||
}
|
||||
|
||||
template<> bool xml_castable<bool>(int XmlType)
|
||||
{
|
||||
return (
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeDouble ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeInt ||
|
||||
XmlType == XmlRpc::XmlRpcValue::TypeBoolean );
|
||||
}
|
||||
|
||||
template<> double xml_cast(XmlRpc::XmlRpcValue xml_value)
|
||||
{
|
||||
using namespace XmlRpc;
|
||||
switch(xml_value.getType()) {
|
||||
case XmlRpcValue::TypeDouble:
|
||||
return static_cast<double>(xml_value);
|
||||
case XmlRpcValue::TypeInt:
|
||||
return static_cast<double>(static_cast<int>(xml_value));
|
||||
case XmlRpcValue::TypeBoolean:
|
||||
return static_cast<double>(static_cast<bool>(xml_value));
|
||||
default:
|
||||
return 0.0;
|
||||
};
|
||||
}
|
||||
|
||||
template<> float xml_cast(XmlRpc::XmlRpcValue xml_value)
|
||||
{
|
||||
using namespace XmlRpc;
|
||||
switch(xml_value.getType()) {
|
||||
case XmlRpcValue::TypeDouble:
|
||||
return static_cast<float>(static_cast<double>(xml_value));
|
||||
case XmlRpcValue::TypeInt:
|
||||
return static_cast<float>(static_cast<int>(xml_value));
|
||||
case XmlRpcValue::TypeBoolean:
|
||||
return static_cast<float>(static_cast<bool>(xml_value));
|
||||
default:
|
||||
return 0.0f;
|
||||
};
|
||||
}
|
||||
|
||||
template<> int xml_cast(XmlRpc::XmlRpcValue xml_value)
|
||||
{
|
||||
using namespace XmlRpc;
|
||||
switch(xml_value.getType()) {
|
||||
case XmlRpcValue::TypeDouble:
|
||||
return static_cast<int>(static_cast<double>(xml_value));
|
||||
case XmlRpcValue::TypeInt:
|
||||
return static_cast<int>(xml_value);
|
||||
case XmlRpcValue::TypeBoolean:
|
||||
return static_cast<int>(static_cast<bool>(xml_value));
|
||||
default:
|
||||
return 0;
|
||||
};
|
||||
}
|
||||
|
||||
template<> bool xml_cast(XmlRpc::XmlRpcValue xml_value)
|
||||
{
|
||||
using namespace XmlRpc;
|
||||
switch(xml_value.getType()) {
|
||||
case XmlRpcValue::TypeDouble:
|
||||
return static_cast<bool>(static_cast<double>(xml_value));
|
||||
case XmlRpcValue::TypeInt:
|
||||
return static_cast<bool>(static_cast<int>(xml_value));
|
||||
case XmlRpcValue::TypeBoolean:
|
||||
return static_cast<bool>(xml_value);
|
||||
default:
|
||||
return false;
|
||||
};
|
||||
}
|
||||
|
||||
template <class T>
|
||||
bool getImpl(const std::string& key, std::vector<T>& vec, bool cached)
|
||||
{
|
||||
XmlRpc::XmlRpcValue xml_array;
|
||||
if(!getImpl(key, xml_array, cached)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Make sure it's an array type
|
||||
if(xml_array.getType() != XmlRpc::XmlRpcValue::TypeArray) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Resize the target vector (destructive)
|
||||
vec.resize(xml_array.size());
|
||||
|
||||
// Fill the vector with stuff
|
||||
for (int i = 0; i < xml_array.size(); i++) {
|
||||
if(!xml_castable<T>(xml_array[i].getType())) {
|
||||
return false;
|
||||
}
|
||||
|
||||
vec[i] = xml_cast<T>(xml_array[i]);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool get(const std::string& key, std::vector<std::string>& vec)
|
||||
{
|
||||
return getImpl(key, vec, false);
|
||||
}
|
||||
bool get(const std::string& key, std::vector<double>& vec)
|
||||
{
|
||||
return getImpl(key, vec, false);
|
||||
}
|
||||
bool get(const std::string& key, std::vector<float>& vec)
|
||||
{
|
||||
return getImpl(key, vec, false);
|
||||
}
|
||||
bool get(const std::string& key, std::vector<int>& vec)
|
||||
{
|
||||
return getImpl(key, vec, false);
|
||||
}
|
||||
bool get(const std::string& key, std::vector<bool>& vec)
|
||||
{
|
||||
return getImpl(key, vec, false);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, std::vector<std::string>& vec)
|
||||
{
|
||||
return getImpl(key, vec, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::vector<double>& vec)
|
||||
{
|
||||
return getImpl(key, vec, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::vector<float>& vec)
|
||||
{
|
||||
return getImpl(key, vec, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::vector<int>& vec)
|
||||
{
|
||||
return getImpl(key, vec, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::vector<bool>& vec)
|
||||
{
|
||||
return getImpl(key, vec, true);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
bool getImpl(const std::string& key, std::map<std::string, T>& map, bool cached)
|
||||
{
|
||||
XmlRpc::XmlRpcValue xml_value;
|
||||
if(!getImpl(key, xml_value, cached)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Make sure it's a struct type
|
||||
if(xml_value.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Fill the map with stuff
|
||||
for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = xml_value.begin();
|
||||
it != xml_value.end();
|
||||
++it)
|
||||
{
|
||||
// Make sure this element is the right type
|
||||
if(!xml_castable<T>(it->second.getType())) {
|
||||
return false;
|
||||
}
|
||||
// Store the element
|
||||
map[it->first] = xml_cast<T>(it->second);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool get(const std::string& key, std::map<std::string, std::string>& map)
|
||||
{
|
||||
return getImpl(key, map, false);
|
||||
}
|
||||
bool get(const std::string& key, std::map<std::string, double>& map)
|
||||
{
|
||||
return getImpl(key, map, false);
|
||||
}
|
||||
bool get(const std::string& key, std::map<std::string, float>& map)
|
||||
{
|
||||
return getImpl(key, map, false);
|
||||
}
|
||||
bool get(const std::string& key, std::map<std::string, int>& map)
|
||||
{
|
||||
return getImpl(key, map, false);
|
||||
}
|
||||
bool get(const std::string& key, std::map<std::string, bool>& map)
|
||||
{
|
||||
return getImpl(key, map, false);
|
||||
}
|
||||
|
||||
bool getCached(const std::string& key, std::map<std::string, std::string>& map)
|
||||
{
|
||||
return getImpl(key, map, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::map<std::string, double>& map)
|
||||
{
|
||||
return getImpl(key, map, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::map<std::string, float>& map)
|
||||
{
|
||||
return getImpl(key, map, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::map<std::string, int>& map)
|
||||
{
|
||||
return getImpl(key, map, true);
|
||||
}
|
||||
bool getCached(const std::string& key, std::map<std::string, bool>& map)
|
||||
{
|
||||
return getImpl(key, map, true);
|
||||
}
|
||||
|
||||
bool getParamNames(std::vector<std::string>& keys)
|
||||
{
|
||||
XmlRpc::XmlRpcValue params, result, payload;
|
||||
params[0] = this_node::getName();
|
||||
if (!master::execute("getParamNames", params, result, payload, false)) {
|
||||
return false;
|
||||
}
|
||||
// Make sure it's an array type
|
||||
if (result.getType() != XmlRpc::XmlRpcValue::TypeArray) {
|
||||
return false;
|
||||
}
|
||||
// Make sure it returned 3 elements
|
||||
if (result.size() != 3) {
|
||||
return false;
|
||||
}
|
||||
// Get the actual parameter keys
|
||||
XmlRpc::XmlRpcValue parameters = result[2];
|
||||
// Resize the output
|
||||
keys.resize(parameters.size());
|
||||
|
||||
// Fill the output vector with the answer
|
||||
for (int i = 0; i < parameters.size(); ++i) {
|
||||
if (parameters[i].getType() != XmlRpc::XmlRpcValue::TypeString) {
|
||||
return false;
|
||||
}
|
||||
keys[i] = std::string(parameters[i]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool search(const std::string& key, std::string& result_out)
|
||||
{
|
||||
return search(this_node::getName(), key, result_out);
|
||||
}
|
||||
|
||||
bool search(const std::string& ns, const std::string& key, std::string& result_out)
|
||||
{
|
||||
XmlRpc::XmlRpcValue params, result, payload;
|
||||
params[0] = ns;
|
||||
|
||||
// searchParam needs a separate form of remapping -- remapping on the unresolved name, rather than the
|
||||
// resolved one.
|
||||
|
||||
std::string remapped = key;
|
||||
M_string::const_iterator it = names::getUnresolvedRemappings().find(key);
|
||||
if (it != names::getUnresolvedRemappings().end())
|
||||
{
|
||||
remapped = it->second;
|
||||
}
|
||||
|
||||
params[1] = remapped;
|
||||
// We don't loop here, because validateXmlrpcResponse() returns false
|
||||
// both when we can't contact the master and when the master says, "I
|
||||
// don't have that param."
|
||||
if (!master::execute("searchParam", params, result, payload, false))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
result_out = (std::string)payload;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void update(const std::string& key, const XmlRpc::XmlRpcValue& v)
|
||||
{
|
||||
std::string clean_key = names::clean(key);
|
||||
ROS_DEBUG_NAMED("cached_parameters", "Received parameter update for key [%s]", clean_key.c_str());
|
||||
|
||||
boost::mutex::scoped_lock lock(g_params_mutex);
|
||||
|
||||
if (g_subscribed_params.find(clean_key) != g_subscribed_params.end())
|
||||
{
|
||||
g_params[clean_key] = v;
|
||||
}
|
||||
invalidateParentParams(clean_key);
|
||||
}
|
||||
|
||||
void paramUpdateCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
|
||||
{
|
||||
result[0] = 1;
|
||||
result[1] = std::string("");
|
||||
result[2] = 0;
|
||||
|
||||
ros::param::update((std::string)params[1], params[2]);
|
||||
}
|
||||
|
||||
void init(const M_string& remappings)
|
||||
{
|
||||
M_string::const_iterator it = remappings.begin();
|
||||
M_string::const_iterator end = remappings.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const std::string& name = it->first;
|
||||
const std::string& param = it->second;
|
||||
|
||||
if (name.size() < 2)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (name[0] == '_' && name[1] != '_')
|
||||
{
|
||||
std::string local_name = "~" + name.substr(1);
|
||||
|
||||
bool success = false;
|
||||
|
||||
try
|
||||
{
|
||||
int32_t i = boost::lexical_cast<int32_t>(param);
|
||||
ros::param::set(names::resolve(local_name), i);
|
||||
success = true;
|
||||
}
|
||||
catch (boost::bad_lexical_cast&)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
if (success)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
double d = boost::lexical_cast<double>(param);
|
||||
ros::param::set(names::resolve(local_name), d);
|
||||
success = true;
|
||||
}
|
||||
catch (boost::bad_lexical_cast&)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
if (success)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (param == "true" || param == "True" || param == "TRUE")
|
||||
{
|
||||
ros::param::set(names::resolve(local_name), true);
|
||||
}
|
||||
else if (param == "false" || param == "False" || param == "FALSE")
|
||||
{
|
||||
ros::param::set(names::resolve(local_name), false);
|
||||
}
|
||||
else
|
||||
{
|
||||
ros::param::set(names::resolve(local_name), param);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
XMLRPCManager::instance()->bind("paramUpdate", paramUpdateCallback);
|
||||
}
|
||||
|
||||
} // namespace param
|
||||
|
||||
} // namespace ros
|
||||
104
thirdparty/ros/ros_comm/clients/roscpp/src/libros/poll_manager.cpp
vendored
Normal file
104
thirdparty/ros/ros_comm/clients/roscpp/src/libros/poll_manager.cpp
vendored
Normal file
@@ -0,0 +1,104 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names 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.
|
||||
*/
|
||||
|
||||
#include "ros/poll_manager.h"
|
||||
#include "ros/common.h"
|
||||
|
||||
#include <signal.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
const PollManagerPtr& PollManager::instance()
|
||||
{
|
||||
static PollManagerPtr poll_manager = boost::make_shared<PollManager>();
|
||||
return poll_manager;
|
||||
}
|
||||
|
||||
PollManager::PollManager()
|
||||
: shutting_down_(false)
|
||||
{
|
||||
}
|
||||
|
||||
PollManager::~PollManager()
|
||||
{
|
||||
shutdown();
|
||||
}
|
||||
|
||||
void PollManager::start()
|
||||
{
|
||||
shutting_down_ = false;
|
||||
thread_ = boost::thread(&PollManager::threadFunc, this);
|
||||
}
|
||||
|
||||
void PollManager::shutdown()
|
||||
{
|
||||
if (shutting_down_) return;
|
||||
|
||||
shutting_down_ = true;
|
||||
if (thread_.get_id() != boost::this_thread::get_id())
|
||||
{
|
||||
thread_.join();
|
||||
}
|
||||
|
||||
boost::recursive_mutex::scoped_lock lock(signal_mutex_);
|
||||
poll_signal_.disconnect_all_slots();
|
||||
}
|
||||
|
||||
void PollManager::threadFunc()
|
||||
{
|
||||
disableAllSignalsInThisThread();
|
||||
|
||||
while (!shutting_down_)
|
||||
{
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(signal_mutex_);
|
||||
poll_signal_();
|
||||
}
|
||||
|
||||
if (shutting_down_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
poll_set_.update(100);
|
||||
}
|
||||
}
|
||||
|
||||
boost::signals2::connection PollManager::addPollThreadListener(const VoidFunc& func)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(signal_mutex_);
|
||||
return poll_signal_.connect(func);
|
||||
}
|
||||
|
||||
void PollManager::removePollThreadListener(boost::signals2::connection c)
|
||||
{
|
||||
boost::recursive_mutex::scoped_lock lock(signal_mutex_);
|
||||
c.disconnect();
|
||||
}
|
||||
|
||||
}
|
||||
300
thirdparty/ros/ros_comm/clients/roscpp/src/libros/poll_set.cpp
vendored
Normal file
300
thirdparty/ros/ros_comm/clients/roscpp/src/libros/poll_set.cpp
vendored
Normal file
@@ -0,0 +1,300 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "ros/poll_set.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#include "ros/transport/transport.h"
|
||||
|
||||
#include <ros/assert.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <fcntl.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
PollSet::PollSet()
|
||||
: sockets_changed_(false), epfd_(create_socket_watcher())
|
||||
{
|
||||
if ( create_signal_pair(signal_pipe_) != 0 ) {
|
||||
ROS_FATAL("create_signal_pair() failed");
|
||||
ROS_BREAK();
|
||||
}
|
||||
addSocket(signal_pipe_[0], boost::bind(&PollSet::onLocalPipeEvents, this, _1));
|
||||
addEvents(signal_pipe_[0], POLLIN);
|
||||
}
|
||||
|
||||
PollSet::~PollSet()
|
||||
{
|
||||
close_signal_pair(signal_pipe_);
|
||||
close_socket_watcher(epfd_);
|
||||
}
|
||||
|
||||
bool PollSet::addSocket(int fd, const SocketUpdateFunc& update_func, const TransportPtr& transport)
|
||||
{
|
||||
SocketInfo info;
|
||||
info.fd_ = fd;
|
||||
info.events_ = 0;
|
||||
info.transport_ = transport;
|
||||
info.func_ = update_func;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(socket_info_mutex_);
|
||||
|
||||
bool b = socket_info_.insert(std::make_pair(fd, info)).second;
|
||||
if (!b)
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("PollSet: Tried to add duplicate fd [%d]", fd);
|
||||
return false;
|
||||
}
|
||||
|
||||
add_socket_to_watcher(epfd_, fd);
|
||||
|
||||
sockets_changed_ = true;
|
||||
}
|
||||
|
||||
signal();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PollSet::delSocket(int fd)
|
||||
{
|
||||
if(fd < 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
boost::mutex::scoped_lock lock(socket_info_mutex_);
|
||||
M_SocketInfo::iterator it = socket_info_.find(fd);
|
||||
if (it != socket_info_.end())
|
||||
{
|
||||
socket_info_.erase(it);
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(just_deleted_mutex_);
|
||||
just_deleted_.push_back(fd);
|
||||
}
|
||||
|
||||
del_socket_from_watcher(epfd_, fd);
|
||||
|
||||
sockets_changed_ = true;
|
||||
signal();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ROSCPP_LOG_DEBUG("PollSet: Tried to delete fd [%d] which is not being tracked", fd);
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
bool PollSet::addEvents(int sock, int events)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(socket_info_mutex_);
|
||||
|
||||
M_SocketInfo::iterator it = socket_info_.find(sock);
|
||||
|
||||
if (it == socket_info_.end())
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("PollSet: Tried to add events [%d] to fd [%d] which does not exist in this pollset", events, sock);
|
||||
return false;
|
||||
}
|
||||
|
||||
it->second.events_ |= events;
|
||||
|
||||
set_events_on_socket(epfd_, sock, it->second.events_);
|
||||
|
||||
sockets_changed_ = true;
|
||||
signal();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PollSet::delEvents(int sock, int events)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(socket_info_mutex_);
|
||||
|
||||
M_SocketInfo::iterator it = socket_info_.find(sock);
|
||||
if (it != socket_info_.end())
|
||||
{
|
||||
it->second.events_ &= ~events;
|
||||
}
|
||||
else
|
||||
{
|
||||
ROSCPP_LOG_DEBUG("PollSet: Tried to delete events [%d] to fd [%d] which does not exist in this pollset", events, sock);
|
||||
return false;
|
||||
}
|
||||
|
||||
set_events_on_socket(epfd_, sock, it->second.events_);
|
||||
|
||||
sockets_changed_ = true;
|
||||
signal();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PollSet::signal()
|
||||
{
|
||||
boost::mutex::scoped_try_lock lock(signal_mutex_);
|
||||
|
||||
if (lock.owns_lock())
|
||||
{
|
||||
char b = 0;
|
||||
if (write_signal(signal_pipe_[1], &b, 1) < 0)
|
||||
{
|
||||
// do nothing... this prevents warnings on gcc 4.3
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void PollSet::update(int poll_timeout)
|
||||
{
|
||||
createNativePollset();
|
||||
|
||||
// Poll across the sockets we're servicing
|
||||
boost::shared_ptr<std::vector<socket_pollfd> > ofds = poll_sockets(epfd_, &ufds_.front(), ufds_.size(), poll_timeout);
|
||||
if (!ofds)
|
||||
{
|
||||
ROS_ERROR_STREAM("poll failed with error " << last_socket_error_string());
|
||||
}
|
||||
else
|
||||
{
|
||||
for (std::vector<socket_pollfd>::iterator it = ofds->begin() ; it != ofds->end(); ++it)
|
||||
{
|
||||
int fd = it->fd;
|
||||
int revents = it->revents;
|
||||
SocketUpdateFunc func;
|
||||
TransportPtr transport;
|
||||
int events = 0;
|
||||
|
||||
if (revents == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
{
|
||||
boost::mutex::scoped_lock lock(socket_info_mutex_);
|
||||
M_SocketInfo::iterator it = socket_info_.find(fd);
|
||||
// the socket has been entirely deleted
|
||||
if (it == socket_info_.end())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
const SocketInfo& info = it->second;
|
||||
|
||||
// Store off the function and transport in case the socket is deleted from another thread
|
||||
func = info.func_;
|
||||
transport = info.transport_;
|
||||
events = info.events_;
|
||||
}
|
||||
|
||||
// If these are registered events for this socket, OR the events are ERR/HUP/NVAL,
|
||||
// call through to the registered function
|
||||
if (func
|
||||
&& ((events & revents)
|
||||
|| (revents & POLLERR)
|
||||
|| (revents & POLLHUP)
|
||||
|| (revents & POLLNVAL)))
|
||||
{
|
||||
bool skip = false;
|
||||
if (revents & (POLLNVAL|POLLERR|POLLHUP))
|
||||
{
|
||||
// If a socket was just closed and then the file descriptor immediately reused, we can
|
||||
// get in here with what we think is a valid socket (since it was just re-added to our set)
|
||||
// but which is actually referring to the previous fd with the same #. If this is the case,
|
||||
// we ignore the first instance of one of these errors. If it's a real error we'll
|
||||
// hit it again next time through.
|
||||
boost::mutex::scoped_lock lock(just_deleted_mutex_);
|
||||
if (std::find(just_deleted_.begin(), just_deleted_.end(), fd) != just_deleted_.end())
|
||||
{
|
||||
skip = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!skip)
|
||||
{
|
||||
func(revents & (events|POLLERR|POLLHUP|POLLNVAL));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
boost::mutex::scoped_lock lock(just_deleted_mutex_);
|
||||
just_deleted_.clear();
|
||||
|
||||
}
|
||||
|
||||
void PollSet::createNativePollset()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(socket_info_mutex_);
|
||||
|
||||
if (!sockets_changed_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Build the list of structures to pass to poll for the sockets we're servicing
|
||||
ufds_.resize(socket_info_.size());
|
||||
M_SocketInfo::iterator sock_it = socket_info_.begin();
|
||||
M_SocketInfo::iterator sock_end = socket_info_.end();
|
||||
for (int i = 0; sock_it != sock_end; ++sock_it, ++i)
|
||||
{
|
||||
const SocketInfo& info = sock_it->second;
|
||||
socket_pollfd& pfd = ufds_[i];
|
||||
pfd.fd = info.fd_;
|
||||
pfd.events = info.events_;
|
||||
pfd.revents = 0;
|
||||
}
|
||||
sockets_changed_ = false;
|
||||
}
|
||||
|
||||
void PollSet::onLocalPipeEvents(int events)
|
||||
{
|
||||
if(events & POLLIN)
|
||||
{
|
||||
char b;
|
||||
while(read_signal(signal_pipe_[0], &b, 1) > 0)
|
||||
{
|
||||
//do nothing keep draining
|
||||
};
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
507
thirdparty/ros/ros_comm/clients/roscpp/src/libros/publication.cpp
vendored
Normal file
507
thirdparty/ros/ros_comm/clients/roscpp/src/libros/publication.cpp
vendored
Normal file
@@ -0,0 +1,507 @@
|
||||
/*
|
||||
* Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include "ros/publication.h"
|
||||
#include "ros/subscriber_link.h"
|
||||
#include "ros/connection.h"
|
||||
#include "ros/callback_queue_interface.h"
|
||||
#include "ros/single_subscriber_publisher.h"
|
||||
#include "ros/serialization.h"
|
||||
#include <std_msgs/Header.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
class PeerConnDisconnCallback : public CallbackInterface
|
||||
{
|
||||
public:
|
||||
PeerConnDisconnCallback(const SubscriberStatusCallback& callback, const SubscriberLinkPtr& sub_link, bool use_tracked_object, const VoidConstWPtr& tracked_object)
|
||||
: callback_(callback)
|
||||
, sub_link_(sub_link)
|
||||
, use_tracked_object_(use_tracked_object)
|
||||
, tracked_object_(tracked_object)
|
||||
{
|
||||
}
|
||||
|
||||
virtual CallResult call()
|
||||
{
|
||||
VoidConstPtr tracker;
|
||||
if (use_tracked_object_)
|
||||
{
|
||||
tracker = tracked_object_.lock();
|
||||
|
||||
if (!tracker)
|
||||
{
|
||||
return Invalid;
|
||||
}
|
||||
}
|
||||
|
||||
SingleSubscriberPublisher pub(sub_link_);
|
||||
callback_(pub);
|
||||
|
||||
return Success;
|
||||
}
|
||||
|
||||
private:
|
||||
SubscriberStatusCallback callback_;
|
||||
SubscriberLinkPtr sub_link_;
|
||||
bool use_tracked_object_;
|
||||
VoidConstWPtr tracked_object_;
|
||||
};
|
||||
|
||||
Publication::Publication(const std::string &name,
|
||||
const std::string &datatype,
|
||||
const std::string &_md5sum,
|
||||
const std::string& message_definition,
|
||||
size_t max_queue,
|
||||
bool latch,
|
||||
bool has_header)
|
||||
: name_(name),
|
||||
datatype_(datatype),
|
||||
md5sum_(_md5sum),
|
||||
message_definition_(message_definition),
|
||||
max_queue_(max_queue),
|
||||
seq_(0),
|
||||
dropped_(false),
|
||||
latch_(latch),
|
||||
has_header_(has_header),
|
||||
intraprocess_subscriber_count_(0)
|
||||
{
|
||||
}
|
||||
|
||||
Publication::~Publication()
|
||||
{
|
||||
drop();
|
||||
}
|
||||
|
||||
void Publication::addCallbacks(const SubscriberCallbacksPtr& callbacks)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(callbacks_mutex_);
|
||||
|
||||
callbacks_.push_back(callbacks);
|
||||
|
||||
// Add connect callbacks for all current subscriptions if this publisher wants them
|
||||
if (callbacks->connect_ && callbacks->callback_queue_)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
V_SubscriberLink::iterator it = subscriber_links_.begin();
|
||||
V_SubscriberLink::iterator end = subscriber_links_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const SubscriberLinkPtr& sub_link = *it;
|
||||
CallbackInterfacePtr cb(boost::make_shared<PeerConnDisconnCallback>(callbacks->connect_, sub_link, callbacks->has_tracked_object_, callbacks->tracked_object_));
|
||||
callbacks->callback_queue_->addCallback(cb, (uint64_t)callbacks.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Publication::removeCallbacks(const SubscriberCallbacksPtr& callbacks)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(callbacks_mutex_);
|
||||
|
||||
V_Callback::iterator it = std::find(callbacks_.begin(), callbacks_.end(), callbacks);
|
||||
if (it != callbacks_.end())
|
||||
{
|
||||
const SubscriberCallbacksPtr& cb = *it;
|
||||
if (cb->callback_queue_)
|
||||
{
|
||||
cb->callback_queue_->removeByID((uint64_t)cb.get());
|
||||
}
|
||||
callbacks_.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
void Publication::drop()
|
||||
{
|
||||
// grab a lock here, to ensure that no subscription callback will
|
||||
// be invoked after we return
|
||||
{
|
||||
boost::mutex::scoped_lock lock(publish_queue_mutex_);
|
||||
boost::mutex::scoped_lock lock2(subscriber_links_mutex_);
|
||||
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
dropped_ = true;
|
||||
}
|
||||
|
||||
dropAllConnections();
|
||||
}
|
||||
|
||||
bool Publication::enqueueMessage(const SerializedMessage& m)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
if (dropped_)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
ROS_ASSERT(m.buf);
|
||||
|
||||
uint32_t seq = incrementSequence();
|
||||
if (has_header_)
|
||||
{
|
||||
// If we have a header, we know it's immediately after the message length
|
||||
// Deserialize it, write the sequence, and then serialize it again.
|
||||
namespace ser = ros::serialization;
|
||||
std_msgs::Header header;
|
||||
ser::IStream istream(m.buf.get() + 4, m.num_bytes - 4);
|
||||
ser::deserialize(istream, header);
|
||||
header.seq = seq;
|
||||
ser::OStream ostream(m.buf.get() + 4, m.num_bytes - 4);
|
||||
ser::serialize(ostream, header);
|
||||
}
|
||||
|
||||
for(V_SubscriberLink::iterator i = subscriber_links_.begin();
|
||||
i != subscriber_links_.end(); ++i)
|
||||
{
|
||||
const SubscriberLinkPtr& sub_link = (*i);
|
||||
sub_link->enqueueMessage(m, true, false);
|
||||
}
|
||||
|
||||
if (latch_)
|
||||
{
|
||||
last_message_ = m;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Publication::addSubscriberLink(const SubscriberLinkPtr& sub_link)
|
||||
{
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
subscriber_links_.push_back(sub_link);
|
||||
|
||||
if (sub_link->isIntraprocess())
|
||||
{
|
||||
++intraprocess_subscriber_count_;
|
||||
}
|
||||
}
|
||||
|
||||
if (latch_ && last_message_.buf)
|
||||
{
|
||||
sub_link->enqueueMessage(last_message_, true, true);
|
||||
}
|
||||
|
||||
// This call invokes the subscribe callback if there is one.
|
||||
// This must happen *after* the push_back above, in case the
|
||||
// callback uses publish().
|
||||
peerConnect(sub_link);
|
||||
}
|
||||
|
||||
void Publication::removeSubscriberLink(const SubscriberLinkPtr& sub_link)
|
||||
{
|
||||
SubscriberLinkPtr link;
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (sub_link->isIntraprocess())
|
||||
{
|
||||
--intraprocess_subscriber_count_;
|
||||
}
|
||||
|
||||
V_SubscriberLink::iterator it = std::find(subscriber_links_.begin(), subscriber_links_.end(), sub_link);
|
||||
if (it != subscriber_links_.end())
|
||||
{
|
||||
link = *it;
|
||||
subscriber_links_.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
if (link)
|
||||
{
|
||||
peerDisconnect(link);
|
||||
}
|
||||
}
|
||||
|
||||
XmlRpc::XmlRpcValue Publication::getStats()
|
||||
{
|
||||
XmlRpc::XmlRpcValue stats;
|
||||
stats[0] = name_;
|
||||
XmlRpc::XmlRpcValue conn_data;
|
||||
conn_data.setSize(0); // force to be an array, even if it's empty
|
||||
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
|
||||
uint32_t cidx = 0;
|
||||
for (V_SubscriberLink::iterator c = subscriber_links_.begin();
|
||||
c != subscriber_links_.end(); ++c, cidx++)
|
||||
{
|
||||
const SubscriberLink::Stats& s = (*c)->getStats();
|
||||
conn_data[cidx][0] = (*c)->getConnectionID();
|
||||
// todo: figure out what to do here... the bytes_sent will wrap around
|
||||
// on some flows within a reasonable amount of time. xmlrpc++ doesn't
|
||||
// seem to give me a nice way to do 64-bit ints, perhaps that's a
|
||||
// limitation of xml-rpc, not sure. alternatively we could send the number
|
||||
// of KB transmitted to gain a few order of magnitude.
|
||||
conn_data[cidx][1] = (int)s.bytes_sent_;
|
||||
conn_data[cidx][2] = (int)s.message_data_sent_;
|
||||
conn_data[cidx][3] = (int)s.messages_sent_;
|
||||
conn_data[cidx][4] = 0; // not sure what is meant by connected
|
||||
}
|
||||
|
||||
stats[1] = conn_data;
|
||||
return stats;
|
||||
}
|
||||
|
||||
// Publisher : [(connection_id, destination_caller_id, direction, transport, topic_name, connected, connection_info_string)*]
|
||||
// e.g. [(2, '/listener', 'o', 'TCPROS', '/chatter', 1, 'TCPROS connection on port 55878 to [127.0.0.1:44273 on socket 7]')]
|
||||
void Publication::getInfo(XmlRpc::XmlRpcValue& info)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
|
||||
for (V_SubscriberLink::iterator c = subscriber_links_.begin();
|
||||
c != subscriber_links_.end(); ++c)
|
||||
{
|
||||
XmlRpc::XmlRpcValue curr_info;
|
||||
curr_info[0] = (int)(*c)->getConnectionID();
|
||||
curr_info[1] = (*c)->getDestinationCallerID();
|
||||
curr_info[2] = "o";
|
||||
curr_info[3] = (*c)->getTransportType();
|
||||
curr_info[4] = name_;
|
||||
curr_info[5] = true; // For length compatibility with rospy
|
||||
curr_info[6] = (*c)->getTransportInfo();
|
||||
info[info.size()] = curr_info;
|
||||
}
|
||||
}
|
||||
|
||||
void Publication::dropAllConnections()
|
||||
{
|
||||
// Swap our publishers list with a local one so we can only lock for a short period of time, because a
|
||||
// side effect of our calling drop() on connections can be re-locking the publishers mutex
|
||||
V_SubscriberLink local_publishers;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
|
||||
local_publishers.swap(subscriber_links_);
|
||||
}
|
||||
|
||||
for (V_SubscriberLink::iterator i = local_publishers.begin();
|
||||
i != local_publishers.end(); ++i)
|
||||
{
|
||||
(*i)->drop();
|
||||
}
|
||||
}
|
||||
|
||||
void Publication::peerConnect(const SubscriberLinkPtr& sub_link)
|
||||
{
|
||||
V_Callback::iterator it = callbacks_.begin();
|
||||
V_Callback::iterator end = callbacks_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const SubscriberCallbacksPtr& cbs = *it;
|
||||
if (cbs->connect_ && cbs->callback_queue_)
|
||||
{
|
||||
CallbackInterfacePtr cb(boost::make_shared<PeerConnDisconnCallback>(cbs->connect_, sub_link, cbs->has_tracked_object_, cbs->tracked_object_));
|
||||
cbs->callback_queue_->addCallback(cb, (uint64_t)cbs.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Publication::peerDisconnect(const SubscriberLinkPtr& sub_link)
|
||||
{
|
||||
V_Callback::iterator it = callbacks_.begin();
|
||||
V_Callback::iterator end = callbacks_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const SubscriberCallbacksPtr& cbs = *it;
|
||||
if (cbs->disconnect_ && cbs->callback_queue_)
|
||||
{
|
||||
CallbackInterfacePtr cb(boost::make_shared<PeerConnDisconnCallback>(cbs->disconnect_, sub_link, cbs->has_tracked_object_, cbs->tracked_object_));
|
||||
cbs->callback_queue_->addCallback(cb, (uint64_t)cbs.get());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
size_t Publication::getNumCallbacks()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(callbacks_mutex_);
|
||||
return callbacks_.size();
|
||||
}
|
||||
|
||||
uint32_t Publication::incrementSequence()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(seq_mutex_);
|
||||
uint32_t old_seq = seq_;
|
||||
++seq_;
|
||||
|
||||
return old_seq;
|
||||
}
|
||||
|
||||
uint32_t Publication::getNumSubscribers()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
return (uint32_t)subscriber_links_.size();
|
||||
}
|
||||
|
||||
void Publication::getPublishTypes(bool& serialize, bool& nocopy, const std::type_info& ti)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
V_SubscriberLink::const_iterator it = subscriber_links_.begin();
|
||||
V_SubscriberLink::const_iterator end = subscriber_links_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const SubscriberLinkPtr& sub = *it;
|
||||
bool s = false;
|
||||
bool n = false;
|
||||
sub->getPublishTypes(s, n, ti);
|
||||
serialize = serialize || s;
|
||||
nocopy = nocopy || n;
|
||||
|
||||
if (serialize && nocopy)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Publication::hasSubscribers()
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
return !subscriber_links_.empty();
|
||||
}
|
||||
|
||||
void Publication::publish(SerializedMessage& m)
|
||||
{
|
||||
if (m.message)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(subscriber_links_mutex_);
|
||||
V_SubscriberLink::const_iterator it = subscriber_links_.begin();
|
||||
V_SubscriberLink::const_iterator end = subscriber_links_.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
const SubscriberLinkPtr& sub = *it;
|
||||
if (sub->isIntraprocess())
|
||||
{
|
||||
sub->enqueueMessage(m, false, true);
|
||||
}
|
||||
}
|
||||
|
||||
m.message.reset();
|
||||
}
|
||||
|
||||
if (m.buf)
|
||||
{
|
||||
boost::mutex::scoped_lock lock(publish_queue_mutex_);
|
||||
publish_queue_.push_back(m);
|
||||
}
|
||||
}
|
||||
|
||||
void Publication::processPublishQueue()
|
||||
{
|
||||
V_SerializedMessage queue;
|
||||
{
|
||||
boost::mutex::scoped_lock lock(publish_queue_mutex_);
|
||||
|
||||
if (dropped_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
queue.insert(queue.end(), publish_queue_.begin(), publish_queue_.end());
|
||||
publish_queue_.clear();
|
||||
}
|
||||
|
||||
if (queue.empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
V_SerializedMessage::iterator it = queue.begin();
|
||||
V_SerializedMessage::iterator end = queue.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
enqueueMessage(*it);
|
||||
}
|
||||
}
|
||||
|
||||
bool Publication::validateHeader(const Header& header, std::string& error_msg)
|
||||
{
|
||||
std::string md5sum, topic, client_callerid;
|
||||
if (!header.getValue("md5sum", md5sum)
|
||||
|| !header.getValue("topic", topic)
|
||||
|| !header.getValue("callerid", client_callerid))
|
||||
{
|
||||
std::string msg("Header from subscriber did not have the required elements: md5sum, topic, callerid");
|
||||
|
||||
ROS_ERROR("%s", msg.c_str());
|
||||
error_msg = msg;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Check whether the topic has been deleted from
|
||||
// advertised_topics through a call to unadvertise(), which could
|
||||
// have happened while we were waiting for the subscriber to
|
||||
// provide the md5sum.
|
||||
if(isDropped())
|
||||
{
|
||||
std::string msg = std::string("received a tcpros connection for a nonexistent topic [") +
|
||||
topic + std::string("] from [" + client_callerid +"].");
|
||||
|
||||
ROS_ERROR("%s", msg.c_str());
|
||||
error_msg = msg;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
if (getMD5Sum() != md5sum &&
|
||||
(md5sum != std::string("*") && getMD5Sum() != std::string("*")))
|
||||
{
|
||||
std::string datatype;
|
||||
header.getValue("type", datatype);
|
||||
|
||||
std::string msg = std::string("Client [") + client_callerid + std::string("] wants topic ") + topic +
|
||||
std::string(" to have datatype/md5sum [") + datatype + "/" + md5sum +
|
||||
std::string("], but our version has [") + getDataType() + "/" + getMD5Sum() +
|
||||
std::string("]. Dropping connection.");
|
||||
|
||||
ROS_ERROR("%s", msg.c_str());
|
||||
error_msg = msg;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
149
thirdparty/ros/ros_comm/clients/roscpp/src/libros/publisher.cpp
vendored
Normal file
149
thirdparty/ros/ros_comm/clients/roscpp/src/libros/publisher.cpp
vendored
Normal file
@@ -0,0 +1,149 @@
|
||||
/*
|
||||
* Copyright (C) 2009, Willow Garage, Inc.
|
||||
*
|
||||
* 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 names of Stanford University or 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.
|
||||
*/
|
||||
|
||||
#include "ros/publisher.h"
|
||||
#include "ros/publication.h"
|
||||
#include "ros/node_handle.h"
|
||||
#include "ros/topic_manager.h"
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
Publisher::Impl::Impl() : unadvertised_(false) { }
|
||||
|
||||
Publisher::Impl::~Impl()
|
||||
{
|
||||
ROS_DEBUG("Publisher on '%s' deregistering callbacks.", topic_.c_str());
|
||||
unadvertise();
|
||||
}
|
||||
|
||||
bool Publisher::Impl::isValid() const
|
||||
{
|
||||
return !unadvertised_;
|
||||
}
|
||||
|
||||
void Publisher::Impl::unadvertise()
|
||||
{
|
||||
if (!unadvertised_)
|
||||
{
|
||||
unadvertised_ = true;
|
||||
TopicManager::instance()->unadvertise(topic_, callbacks_);
|
||||
node_handle_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
Publisher::Publisher(const std::string& topic, const std::string& md5sum, const std::string& datatype, const NodeHandle& node_handle, const SubscriberCallbacksPtr& callbacks)
|
||||
: impl_(boost::make_shared<Impl>())
|
||||
{
|
||||
impl_->topic_ = topic;
|
||||
impl_->md5sum_ = md5sum;
|
||||
impl_->datatype_ = datatype;
|
||||
impl_->node_handle_ = boost::make_shared<NodeHandle>(node_handle);
|
||||
impl_->callbacks_ = callbacks;
|
||||
}
|
||||
|
||||
Publisher::Publisher(const Publisher& rhs)
|
||||
{
|
||||
impl_ = rhs.impl_;
|
||||
}
|
||||
|
||||
Publisher::~Publisher()
|
||||
{
|
||||
}
|
||||
|
||||
void Publisher::publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) const
|
||||
{
|
||||
if (!impl_)
|
||||
{
|
||||
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
if (!impl_->isValid())
|
||||
{
|
||||
ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str());
|
||||
return;
|
||||
}
|
||||
|
||||
TopicManager::instance()->publish(impl_->topic_, serfunc, m);
|
||||
}
|
||||
|
||||
void Publisher::incrementSequence() const
|
||||
{
|
||||
if (impl_ && impl_->isValid())
|
||||
{
|
||||
TopicManager::instance()->incrementSequence(impl_->topic_);
|
||||
}
|
||||
}
|
||||
|
||||
void Publisher::shutdown()
|
||||
{
|
||||
if (impl_)
|
||||
{
|
||||
impl_->unadvertise();
|
||||
impl_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
std::string Publisher::getTopic() const
|
||||
{
|
||||
if (impl_)
|
||||
{
|
||||
return impl_->topic_;
|
||||
}
|
||||
|
||||
return std::string();
|
||||
}
|
||||
|
||||
uint32_t Publisher::getNumSubscribers() const
|
||||
{
|
||||
if (impl_ && impl_->isValid())
|
||||
{
|
||||
return TopicManager::instance()->getNumSubscribers(impl_->topic_);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool Publisher::isLatched() const {
|
||||
PublicationPtr publication_ptr;
|
||||
if (impl_ && impl_->isValid()) {
|
||||
publication_ptr =
|
||||
TopicManager::instance()->lookupPublication(impl_->topic_);
|
||||
} else {
|
||||
ROS_ASSERT_MSG(false, "Call to isLatched() on an invalid Publisher");
|
||||
throw ros::Exception("Call to isLatched() on an invalid Publisher");
|
||||
}
|
||||
if (publication_ptr) {
|
||||
return publication_ptr->isLatched();
|
||||
} else {
|
||||
ROS_ASSERT_MSG(false, "Call to isLatched() on an invalid Publisher");
|
||||
throw ros::Exception("Call to isLatched() on an invalid Publisher");
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
114
thirdparty/ros/ros_comm/clients/roscpp/src/libros/publisher_link.cpp
vendored
Normal file
114
thirdparty/ros/ros_comm/clients/roscpp/src/libros/publisher_link.cpp
vendored
Normal file
@@ -0,0 +1,114 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "ros/publisher_link.h"
|
||||
#include "ros/subscription.h"
|
||||
#include "ros/header.h"
|
||||
#include "ros/connection.h"
|
||||
#include "ros/transport/transport.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/connection_manager.h"
|
||||
#include "ros/file_log.h"
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
#include <sstream>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
PublisherLink::PublisherLink(const SubscriptionPtr& parent, const std::string& xmlrpc_uri,
|
||||
const TransportHints& transport_hints)
|
||||
: parent_(parent)
|
||||
, connection_id_(0)
|
||||
, publisher_xmlrpc_uri_(xmlrpc_uri)
|
||||
, transport_hints_(transport_hints)
|
||||
, latched_(false)
|
||||
{ }
|
||||
|
||||
PublisherLink::~PublisherLink()
|
||||
{ }
|
||||
|
||||
bool PublisherLink::setHeader(const Header& header)
|
||||
{
|
||||
header.getValue("callerid", caller_id_);
|
||||
|
||||
std::string md5sum, type, latched_str;
|
||||
if (!header.getValue("md5sum", md5sum))
|
||||
{
|
||||
ROS_ERROR("Publisher header did not have required element: md5sum");
|
||||
return false;
|
||||
}
|
||||
|
||||
md5sum_ = md5sum;
|
||||
|
||||
if (!header.getValue("type", type))
|
||||
{
|
||||
ROS_ERROR("Publisher header did not have required element: type");
|
||||
return false;
|
||||
}
|
||||
|
||||
latched_ = false;
|
||||
if (header.getValue("latching", latched_str))
|
||||
{
|
||||
if (latched_str == "1")
|
||||
{
|
||||
latched_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
connection_id_ = ConnectionManager::instance()->getNewConnectionID();
|
||||
header_ = header;
|
||||
|
||||
if (SubscriptionPtr parent = parent_.lock())
|
||||
{
|
||||
parent->headerReceived(shared_from_this(), header);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
const std::string& PublisherLink::getPublisherXMLRPCURI()
|
||||
{
|
||||
return publisher_xmlrpc_uri_;
|
||||
}
|
||||
|
||||
const std::string& PublisherLink::getMD5Sum()
|
||||
{
|
||||
ROS_ASSERT(!md5sum_.empty());
|
||||
return md5sum_;
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
|
||||
150
thirdparty/ros/ros_comm/clients/roscpp/src/libros/rosout_appender.cpp
vendored
Normal file
150
thirdparty/ros/ros_comm/clients/roscpp/src/libros/rosout_appender.cpp
vendored
Normal file
@@ -0,0 +1,150 @@
|
||||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include "ros/rosout_appender.h"
|
||||
#include "ros/this_node.h"
|
||||
#include "ros/node_handle.h"
|
||||
#include "ros/topic_manager.h"
|
||||
#include "ros/advertise_options.h"
|
||||
#include "ros/names.h"
|
||||
|
||||
#include <rosgraph_msgs/Log.h>
|
||||
|
||||
namespace ros
|
||||
{
|
||||
|
||||
ROSOutAppender::ROSOutAppender()
|
||||
: shutting_down_(false)
|
||||
, publish_thread_(boost::bind(&ROSOutAppender::logThread, this))
|
||||
{
|
||||
AdvertiseOptions ops;
|
||||
ops.init<rosgraph_msgs::Log>(names::resolve("/rosout"), 0);
|
||||
ops.latch = true;
|
||||
SubscriberCallbacksPtr cbs(boost::make_shared<SubscriberCallbacks>());
|
||||
TopicManager::instance()->advertise(ops, cbs);
|
||||
}
|
||||
|
||||
ROSOutAppender::~ROSOutAppender()
|
||||
{
|
||||
shutting_down_ = true;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(queue_mutex_);
|
||||
queue_condition_.notify_all();
|
||||
}
|
||||
|
||||
publish_thread_.join();
|
||||
}
|
||||
|
||||
const std::string& ROSOutAppender::getLastError() const
|
||||
{
|
||||
return last_error_;
|
||||
}
|
||||
|
||||
void ROSOutAppender::log(::ros::console::Level level, const char* str, const char* file, const char* function, int line)
|
||||
{
|
||||
rosgraph_msgs::LogPtr msg(boost::make_shared<rosgraph_msgs::Log>());
|
||||
|
||||
msg->header.stamp = ros::Time::now();
|
||||
if (level == ros::console::levels::Debug)
|
||||
{
|
||||
msg->level = rosgraph_msgs::Log::DEBUG;
|
||||
}
|
||||
else if (level == ros::console::levels::Info)
|
||||
{
|
||||
msg->level = rosgraph_msgs::Log::INFO;
|
||||
}
|
||||
else if (level == ros::console::levels::Warn)
|
||||
{
|
||||
msg->level = rosgraph_msgs::Log::WARN;
|
||||
}
|
||||
else if (level == ros::console::levels::Error)
|
||||
{
|
||||
msg->level = rosgraph_msgs::Log::ERROR;
|
||||
}
|
||||
else if (level == ros::console::levels::Fatal)
|
||||
{
|
||||
msg->level = rosgraph_msgs::Log::FATAL;
|
||||
}
|
||||
msg->name = this_node::getName();
|
||||
msg->msg = str;
|
||||
msg->file = file;
|
||||
msg->function = function;
|
||||
msg->line = line;
|
||||
this_node::getAdvertisedTopics(msg->topics);
|
||||
|
||||
if (level == ::ros::console::levels::Fatal || level == ::ros::console::levels::Error)
|
||||
{
|
||||
last_error_ = str;
|
||||
}
|
||||
|
||||
boost::mutex::scoped_lock lock(queue_mutex_);
|
||||
log_queue_.push_back(msg);
|
||||
queue_condition_.notify_all();
|
||||
}
|
||||
|
||||
void ROSOutAppender::logThread()
|
||||
{
|
||||
while (!shutting_down_)
|
||||
{
|
||||
V_Log local_queue;
|
||||
|
||||
{
|
||||
boost::mutex::scoped_lock lock(queue_mutex_);
|
||||
|
||||
if (shutting_down_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
queue_condition_.wait(lock);
|
||||
|
||||
if (shutting_down_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
local_queue.swap(log_queue_);
|
||||
}
|
||||
|
||||
V_Log::iterator it = local_queue.begin();
|
||||
V_Log::iterator end = local_queue.end();
|
||||
for (; it != end; ++it)
|
||||
{
|
||||
TopicManager::instance()->publish(names::resolve("/rosout"), *(*it));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace ros
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user