pax_global_header00006660000000000000000000000064126203743500014514gustar00rootroot0000000000000052 comment=6191577f5e1c170c6c7daa23d6e1ed23a1e50643 ros-ros-comm-1.11.16/000077500000000000000000000000001262037435000142205ustar00rootroot00000000000000ros-ros-comm-1.11.16/.gitignore000066400000000000000000000000111262037435000162000ustar00rootroot00000000000000*~ *.pyc ros-ros-comm-1.11.16/.travis.yml000066400000000000000000000025671262037435000163430ustar00rootroot00000000000000language: - cpp - python python: - "2.7" compiler: - gcc - clang # command to install dependencies install: - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo apt-get update -qq - sudo apt-get install python-rosdep python-catkin-pkg python-nose python-coverage python-wstool pep8 -qq - sudo apt-get install ros-groovy-catkin -qq - sudo rosdep init - rosdep update # Make a workspace - mkdir -p /tmp/ros_comm_ws/src # Add this folder to the workspace - ln -s `pwd` /tmp/ros_comm_ws/src/ros_comm # Install dependencies - cd /tmp/ros_comm_ws - rosdep install --from-paths src --ignore-src --rosdistro groovy -y > /dev/null # command to run tests script: - source /opt/ros/groovy/setup.bash - cd /tmp/ros_comm_ws - catkin_make tests > log.make_tests - cd build # Manually invoke make, catkin_make - make run_tests > log.run_tests || cat log.* # Check test results - 'python -c "from catkin.test_results import test_results;import sys;sys.exit(sum([v[1] + v[2] for k, v in test_results(\"test_results\").items()]))" || cat log*' - 'python -c "from catkin.test_results import test_results;import sys;sys.exit(sum([v[1] + v[2] for k, v in test_results(\"test_results\").items()]))"' notifications: email: false ros-ros-comm-1.11.16/clients/000077500000000000000000000000001262037435000156615ustar00rootroot00000000000000ros-ros-comm-1.11.16/clients/roscpp/000077500000000000000000000000001262037435000171675ustar00rootroot00000000000000ros-ros-comm-1.11.16/clients/roscpp/CHANGELOG.rst000066400000000000000000000164221262037435000212150ustar00rootroot00000000000000^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Changelog for package roscpp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.11.16 (2015-11-09) -------------------- * add getROSArg function (`#694 `_) 1.11.15 (2015-10-13) -------------------- * fix crash in onRetryTimer() callback (`#577 `_) 1.11.14 (2015-09-19) -------------------- * add optional reset argument to Timer::setPeriod() (`#590 `_) * add getParam() and getParamCached() for float (`#621 `_, `#623 `_) * use explicit bool cast to compile with C++11 (`#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 `_) * fix computation of stddev in statistics (`#556 `_) * fix empty connection header topic (`#543 `_) * alternative API to get parameter values (`#592 `_) * add getCached() for float parameters (`#584 `_) 1.11.10 (2014-12-22) -------------------- * fix various defects reported by coverity * fix comment (`#529 `_) * improve Android support (`#518 `_) 1.11.9 (2014-08-18) ------------------- * add accessor to expose whether service is persistent (`#489 `_) * populate delivered_msgs field of TopicStatistics message (`#486 `_) 1.11.8 (2014-08-04) ------------------- * fix C++11 compatibility issue (`#483 `_) 1.11.7 (2014-07-18) ------------------- * fix segfault due to accessing a NULL pointer for some network interfaces (`#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 `_) 1.11.5 (2014-06-24) ------------------- * improve handling dropped connections (`#434 `_) * add header needed for Android (`#441 `_) * fix typo for parameter used for statistics (`#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 `_) * only update param cache when being subscribed (`#351 `_) * ensure to remove delete parameters completely * invalidate cached parent parameters when namespace parameter is set / changes (`#352 `_) * add optional topic/connection statistics (`#398 `_) * add transport information in SlaveAPI::getBusInfo() for roscpp & rospy (`#328 `_) * add AsyncSpinner::canStart() to check if a spinner can be started 1.11.0 (2014-03-04) ------------------- * allow getting parameters with name '/' (`#313 `_) * support for /clock remapping (`#359 `_) * suppress boost::signals deprecation warning (`#362 `_) * use catkin_install_python() to install Python scripts (`#361 `_) 1.10.0 (2014-02-11) ------------------- * remove use of __connection header 1.9.54 (2014-01-27) ------------------- * fix return value of pubUpdate() (`#334 `_) * fix handling optional third xml rpc response argument (`#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 `_) * add missing version dependency on roscpp_core stuff (`#299 `_) * remove log4cxx dependency from roscpp, using new agnostic interface from rosconsole * fix compile problem with gcc 4.4 (`#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 `_) 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 `_) * fix missing generation of constant definitions for services (`ros/gencpp#2 `_) * fix restoring thread context when callback throws an exception (`#219 `_) * fix calling PollManager::shutdown() repeatedly (`#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 `_) * fix roscpp service call deadlock (`#149 `_) * fix freezing service calls when returning false (`#168 `_) * fix error message publishing wrong message type (`#178 `_) * fix missing explicit dependency on pthread (`#135 `_) * fix compiler warning about wrong comparison of message md5 hashes (`#165 `_) 1.9.41 (2013-01-24) ------------------- * allow sending data exceeding 2GB in chunks (`#4049 `_) * update getParam() doc (`#1460 `_) * add param::get(float) (`#3754 `_) * update inactive assert when publishing message with md5sum "*", update related tests (`#3714 `_) * fix ros master retry timeout (`#4024 `_) * fix inactive assert when publishing message with wrong type (`#3714 `_) 1.9.40 (2013-01-13) ------------------- 1.9.39 (2012-12-29) ------------------- * first public release for Groovy ros-ros-comm-1.11.16/clients/roscpp/CMakeLists.txt000066400000000000000000000113211262037435000217250ustar00rootroot00000000000000cmake_minimum_required(VERSION 2.8.3) project(roscpp) 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 signals filesystem 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) # Not everybody has (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) # 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 ) add_dependencies(roscpp roscpp_gencpp rosgraph_msgs_gencpp std_msgs_gencpp) 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) ros-ros-comm-1.11.16/clients/roscpp/include/000077500000000000000000000000001262037435000206125ustar00rootroot00000000000000ros-ros-comm-1.11.16/clients/roscpp/include/ros/000077500000000000000000000000001262037435000214155ustar00rootroot00000000000000ros-ros-comm-1.11.16/clients/roscpp/include/ros/advertise_options.h000066400000000000000000000166211262037435000253350ustar00rootroot00000000000000/* * 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 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(); datatype = message_traits::datatype(); message_definition = message_traits::definition(); has_header = message_traits::hasHeader(); } 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 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(topic, queue_size, connect_cb, disconnect_cb); ops.tracked_object = tracked_object; ops.callback_queue = queue; return ops; } }; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/advertise_service_options.h000066400000000000000000000160771262037435000270620ustar00rootroot00000000000000/* * 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 void init(const std::string& _service, const boost::function& _callback) { namespace st = service_traits; namespace mt = message_traits; if (st::md5sum() != st::md5sum()) { 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(); datatype = st::datatype(); req_datatype = mt::datatype(); res_datatype = mt::datatype(); helper = ServiceCallbackHelperPtr(new ServiceCallbackHelperT >(_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 void init(const std::string& _service, const boost::function& _callback) { namespace st = service_traits; namespace mt = message_traits; typedef typename Service::Request Request; typedef typename Service::Response Response; service = _service; md5sum = st::md5sum(); datatype = st::datatype(); req_datatype = mt::datatype(); res_datatype = mt::datatype(); helper = ServiceCallbackHelperPtr(new ServiceCallbackHelperT >(_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 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(); datatype = st::datatype(); req_datatype = mt::datatype(); res_datatype = mt::datatype(); helper = ServiceCallbackHelperPtr(new ServiceCallbackHelperT(_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 static AdvertiseServiceOptions create(const std::string& service, const boost::function& callback, const VoidConstPtr& tracked_object, CallbackQueueInterface* queue) { AdvertiseServiceOptions ops; ops.init(service, callback); ops.tracked_object = tracked_object; ops.callback_queue = queue; return ops; } }; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/callback_queue.h000066400000000000000000000131551262037435000245330ustar00rootroot00000000000000/* * 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 #include "ros/callback_queue_interface.h" #include "ros/time.h" #include "common.h" #include #include #include #include #include #include #include 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 IDInfoPtr; typedef std::map 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 L_CallbackInfo; typedef std::deque 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_; bool enabled_; }; typedef boost::shared_ptr CallbackQueuePtr; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/callback_queue_interface.h000066400000000000000000000063171262037435000265550ustar00rootroot00000000000000/* * 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 #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 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/common.h.in000066400000000000000000000057231262037435000234720ustar00rootroot00000000000000/* * 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 #include #include #include #include "ros/assert.h" #include "ros/forwards.h" #include "ros/serialized_message.h" #include #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 // 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/connection.h000066400000000000000000000251021262037435000237250ustar00rootroot00000000000000/* * 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 #include #include #include #include #include #include #define READ_BUFFER_SIZE (1024*64) namespace ros { class Transport; typedef boost::shared_ptr TransportPtr; class Connection; typedef boost::shared_ptr ConnectionPtr; typedef boost::function&, uint32_t, bool)> ReadFinishedFunc; typedef boost::function WriteFinishedFunc; typedef boost::function 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 { 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=". 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&, 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& buffer, uint32_t size, const WriteFinishedFunc& finished_callback, bool immedate = true); typedef boost::signals2::signal DropSignal; typedef boost::function 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& buffer, uint32_t size, bool success); void onHeaderRead(const ConnectionPtr& conn, const boost::shared_array& 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 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 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 ConnectionPtr; } // namespace ros #endif // ROSCPP_CONNECTION_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/connection_manager.h000066400000000000000000000073351262037435000254270ustar00rootroot00000000000000/* * 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 #include namespace ros { class PollManager; typedef boost::shared_ptr PollManagerPtr; class ConnectionManager; typedef boost::shared_ptr 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 }; } ros-ros-comm-1.11.16/clients/roscpp/include/ros/exceptions.h000066400000000000000000000055631262037435000237600ustar00rootroot00000000000000/* * 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 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/file_log.h000066400000000000000000000037171262037435000233560ustar00rootroot00000000000000/* * 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 #include "common.h" #define ROSCPP_LOG_DEBUG(...) ROS_DEBUG_NAMED("roscpp_internal", __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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/forwards.h000066400000000000000000000152551262037435000234250ustar00rootroot00000000000000/* * 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 #include #include #include #include #include #include #include #include #include #include "exceptions.h" #include "ros/datatypes.h" namespace ros { typedef boost::shared_ptr VoidPtr; typedef boost::weak_ptr VoidWPtr; typedef boost::shared_ptr VoidConstPtr; typedef boost::weak_ptr VoidConstWPtr; class Header; class Transport; typedef boost::shared_ptr TransportPtr; class TransportTCP; typedef boost::shared_ptr TransportTCPPtr; class TransportUDP; typedef boost::shared_ptr TransportUDPPtr; class Connection; typedef boost::shared_ptr ConnectionPtr; typedef std::set S_Connection; typedef std::vector V_Connection; class Publication; typedef boost::shared_ptr PublicationPtr; typedef std::vector V_Publication; class SubscriberLink; typedef boost::shared_ptr SubscriberLinkPtr; typedef std::vector V_SubscriberLink; class Subscription; typedef boost::shared_ptr SubscriptionPtr; typedef boost::weak_ptr SubscriptionWPtr; typedef std::list L_Subscription; typedef std::vector V_Subscription; typedef std::set S_Subscription; class PublisherLink; typedef boost::shared_ptr PublisherLinkPtr; typedef std::vector V_PublisherLink; class ServicePublication; typedef boost::shared_ptr ServicePublicationPtr; typedef std::list L_ServicePublication; typedef std::vector V_ServicePublication; class ServiceServerLink; typedef boost::shared_ptr ServiceServerLinkPtr; typedef std::list L_ServiceServerLink; class Transport; typedef boost::shared_ptr TransportPtr; class NodeHandle; typedef boost::shared_ptr NodeHandlePtr; class SingleSubscriberPublisher; typedef boost::function SubscriberStatusCallback; class CallbackQueue; class CallbackQueueInterface; class CallbackInterface; typedef boost::shared_ptr 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 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 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 WallTimerCallback; class ServiceManager; typedef boost::shared_ptr ServiceManagerPtr; class TopicManager; typedef boost::shared_ptr TopicManagerPtr; class ConnectionManager; typedef boost::shared_ptr ConnectionManagerPtr; class XMLRPCManager; typedef boost::shared_ptr XMLRPCManagerPtr; class PollManager; typedef boost::shared_ptr PollManagerPtr; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/init.h000066400000000000000000000205471262037435000225410ustar00rootroot00000000000000/* * 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 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 > 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/internal_timer_manager.h000066400000000000000000000041411262037435000262740ustar00rootroot00000000000000/* * 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 #include "common.h" namespace ros { template class TimerManager; typedef TimerManager InternalTimerManager; typedef boost::shared_ptr InternalTimerManagerPtr; ROSCPP_DECL void initInternalTimerManager(); ROSCPP_DECL InternalTimerManagerPtr getInternalTimerManager(); } // namespace ros #endif // ROSCPP_INTERNAL_TIMER_MANAGER_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/intraprocess_publisher_link.h000066400000000000000000000062061262037435000274000ustar00rootroot00000000000000/* * 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 namespace ros { class Subscription; typedef boost::shared_ptr SubscriptionPtr; typedef boost::weak_ptr SubscriptionWPtr; class IntraProcessSubscriberLink; typedef boost::shared_ptr 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 IntraProcessPublisherLinkPtr; } // namespace ros #endif // ROSCPP_INTRAPROCESS_PUBLISHER_LINK_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/intraprocess_subscriber_link.h000066400000000000000000000055151262037435000275500ustar00rootroot00000000000000/* * 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 namespace ros { class IntraProcessPublisherLink; typedef boost::shared_ptr 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 IntraProcessSubscriberLinkPtr; } // namespace ros #endif // ROSCPP_INTRAPROCESS_SUBSCRIBER_LINK_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/io.h000066400000000000000000000165631262037435000222100ustar00rootroot00000000000000/* * 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 #include "common.h" #ifdef WIN32 #include // For struct timeval #include // Must be after winsock2.h because MS didn't put proper inclusion guards in their headers. #include #include #include #include // for _getpid #else #include // should get cmake to explicitly check for poll.h? #include #include #include #include #include // getnameinfo in network.cpp #include // sockaddr_in in network.cpp #include // 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 /***************************************************************************** ** 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 int poll_sockets(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]); /***************************************************************************** ** 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_ */ ros-ros-comm-1.11.16/clients/roscpp/include/ros/master.h000066400000000000000000000111741262037435000230650ustar00rootroot00000000000000/* * 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 "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 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 . 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/message.h000066400000000000000000000066751262037435000232300ustar00rootroot00000000000000/* * 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 #include #include #include #include #define ROSCPP_MESSAGE_HAS_DEFINITION namespace ros { typedef std::map 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 Ptr; typedef boost::shared_ptr 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 MessagePtr; typedef boost::shared_ptr 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/message_deserializer.h000066400000000000000000000047721262037435000257660ustar00rootroot00000000000000/* * 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 #include #include namespace ros { class SubscriptionCallbackHelper; typedef boost::shared_ptr SubscriptionCallbackHelperPtr; class ROSCPP_DECL MessageDeserializer { public: MessageDeserializer(const SubscriptionCallbackHelperPtr& helper, const SerializedMessage& m, const boost::shared_ptr& connection_header); VoidConstPtr deserialize(); const boost::shared_ptr& getConnectionHeader() { return connection_header_; } private: SubscriptionCallbackHelperPtr helper_; SerializedMessage serialized_message_; boost::shared_ptr connection_header_; boost::mutex mutex_; VoidConstPtr msg_; }; typedef boost::shared_ptr MessageDeserializerPtr; } #endif // ROSCPP_MESSAGE_DESERIALIZER_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/names.h000066400000000000000000000071421262037435000226750ustar00rootroot00000000000000/* * 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/network.h000066400000000000000000000036561262037435000232710ustar00rootroot00000000000000/* * 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/node_handle.h000066400000000000000000002767541262037435000240530ustar00rootroot00000000000000/* * 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_NODE_HANDLE_H #define ROSCPP_NODE_HANDLE_H #include "ros/forwards.h" #include "ros/publisher.h" #include "ros/subscriber.h" #include "ros/service_server.h" #include "ros/service_client.h" #include "ros/timer.h" #include "ros/rate.h" #include "ros/wall_timer.h" #include "ros/advertise_options.h" #include "ros/advertise_service_options.h" #include "ros/subscribe_options.h" #include "ros/service_client_options.h" #include "ros/timer_options.h" #include "ros/wall_timer_options.h" #include "ros/spinner.h" #include "ros/init.h" #include "common.h" #include #include namespace ros { class NodeHandleBackingCollection; /** * \brief roscpp's interface for creating subscribers, publishers, etc. * * This class is used for writing nodes. It provides a RAII interface * to this process' node, in that when the first NodeHandle is * created, it instantiates everything necessary for this node, and * when the last NodeHandle goes out of scope it shuts down the node. * * NodeHandle uses reference counting internally, and copying a * NodeHandle is very lightweight. * * You must call one of the ros::init functions prior to instantiating * this class. * * The most widely used methods are: * - Setup: * - ros::init() * - Publish / subscribe messaging: * - advertise() * - subscribe() * - RPC services: * - advertiseService() * - serviceClient() * - ros::service::call() * - Parameters: * - getParam() * - setParam() */ class ROSCPP_DECL NodeHandle { public: /** * \brief Constructor * * When a NodeHandle is constructed, it checks to see if the global * node state has already been started. If so, it increments a * global reference count. If not, it starts the node with * ros::start() and sets the reference count to 1. * * \param ns Namespace for this NodeHandle. This acts in addition to any namespace assigned to this ROS node. * eg. If the node's namespace is "/a" and the namespace passed in here is "b", all * topics/services/parameters will be prefixed with "/a/b/" * \param remappings Remappings for this NodeHandle. * \throws InvalidNameException if the namespace is not a valid graph resource name */ NodeHandle(const std::string& ns = std::string(), const M_string& remappings = M_string()); /** * \brief Copy constructor * * When a NodeHandle is copied, it inherits the namespace of the * NodeHandle being copied, and increments the reference count of * the global node state by 1. */ NodeHandle(const NodeHandle& rhs); /** * \brief Parent constructor * * This version of the constructor takes a "parent" NodeHandle, and is equivalent to: \verbatim NodeHandle child(parent.getNamespace() + "/" + ns); \endverbatim * * When a NodeHandle is copied, it inherits the namespace of the * NodeHandle being copied, and increments the reference count of * the global node state by 1. * * \throws InvalidNameException if the namespace is not a valid * graph resource name */ NodeHandle(const NodeHandle& parent, const std::string& ns); /** * \brief Parent constructor * * This version of the constructor takes a "parent" NodeHandle, and is equivalent to: \verbatim NodeHandle child(parent.getNamespace() + "/" + ns, remappings); \endverbatim * * This version also lets you pass in name remappings that are specific to this NodeHandle * * When a NodeHandle is copied, it inherits the namespace of the NodeHandle being copied, * and increments the reference count of the global node state * by 1. * \throws InvalidNameException if the namespace is not a valid graph resource name */ NodeHandle(const NodeHandle& parent, const std::string& ns, const M_string& remappings); /** * \brief Destructor * * When a NodeHandle is destroyed, it decrements a global reference * count by 1, and if the reference count is now 0, shuts down the * node. */ ~NodeHandle(); NodeHandle& operator=(const NodeHandle& rhs); /** * \brief Set the default callback queue to be used by this NodeHandle. * * Setting this will cause any callbacks from * advertisements/subscriptions/services/etc. to happen through the * use of the specified queue. NULL (the default) causes the global * queue (serviced by ros::spin() and ros::spinOnce()) to be used. */ void setCallbackQueue(CallbackQueueInterface* queue); /** * \brief Returns the callback queue associated with this * NodeHandle. If none has been explicitly set, returns the global * queue. */ CallbackQueueInterface* getCallbackQueue() const { return callback_queue_ ? callback_queue_ : (CallbackQueueInterface*)getGlobalCallbackQueue(); } /** * \brief Returns the namespace associated with this NodeHandle */ const std::string& getNamespace() const { return namespace_; } /** * \brief Returns the namespace associated with this NodeHandle as * it was passed in (before it was resolved) */ const std::string& getUnresolvedNamespace() const { return unresolved_namespace_; } /** \brief Resolves a name into a fully-qualified name * * Resolves a name into a fully qualified name, eg. "blah" => * "/namespace/blah". By default also applies any matching * name-remapping rules (which were usually supplied on the command * line at startup) to the given name, returning the resulting * remapped name. * * \param name Name to remap * * \param remap Whether to apply name-remapping rules * * \return Resolved name. * * \throws InvalidNameException If the name begins with a tilde, or is an otherwise invalid graph resource name */ std::string resolveName(const std::string& name, bool remap = true) const; ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Versions of advertise() ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * \brief Advertise a topic, simple version * * This call connects to the master to publicize that the node will be * publishing messages on the given topic. This method returns a Publisher that allows you to * publish a message on this topic. * * This version of advertise is a templated convenience function, and can be used like so * * ros::Publisher pub = handle.advertise("my_topic", 1); * * \param topic Topic to advertise on * * \param queue_size Maximum number of outgoing messages to be * queued for delivery to subscribers * * \param latch (optional) If true, the last message published on * this topic will be saved and sent to new subscribers when they * connect * * \return On success, a Publisher that, when it goes out of scope, * will automatically release a reference on this advertisement. On * failure, an empty Publisher. * * \throws InvalidNameException If the topic name begins with a * tilde, or is an otherwise invalid graph resource name, or is an * otherwise invalid graph resource name */ template Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false) { AdvertiseOptions ops; ops.template init(topic, queue_size); ops.latch = latch; return advertise(ops); } /** * \brief Advertise a topic, with most of the available options, including subscriber status callbacks * * This call connects to the master to publicize that the node will be * publishing messages on the given topic. This method returns a Publisher that allows you to * publish a message on this topic. * * This version of advertise allows you to pass functions to be called when new subscribers connect and * disconnect. With bare functions it can be used like so: \verbatim void connectCallback(const ros::SingleSubscriberPublisher& pub) { // Do something } handle.advertise("my_topic", 1, (ros::SubscriberStatusCallback)connectCallback); \endverbatim * * With class member functions it can be used with boost::bind: \verbatim void MyClass::connectCallback(const ros::SingleSubscriberPublisher& pub) { // Do something } MyClass my_class; ros::Publisher pub = handle.advertise("my_topic", 1, boost::bind(&MyClass::connectCallback, my_class, _1)); \endverbatim * * * \param topic Topic to advertise 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 * * \param disconnect_cb (optional) Function to call when a subscriber disconnects * * \param tracked_object (optional) 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 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. * \param latch (optional) If true, the last message published on this topic will be saved and sent to new subscribers when they connect * \return On success, a Publisher that, when it goes out of scope, will automatically release a reference * on this advertisement. On failure, an empty Publisher which can be checked with: \verbatim ros::NodeHandle nodeHandle; ros::publisher pub = nodeHandle.advertise("my_topic", 1, (ros::SubscriberStatusCallback)callback); if (pub) // Enter if publisher is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name */ template Publisher advertise(const std::string& topic, uint32_t queue_size, const SubscriberStatusCallback& connect_cb, const SubscriberStatusCallback& disconnect_cb = SubscriberStatusCallback(), const VoidConstPtr& tracked_object = VoidConstPtr(), bool latch = false) { AdvertiseOptions ops; ops.template init(topic, queue_size, connect_cb, disconnect_cb); ops.tracked_object = tracked_object; ops.latch = latch; return advertise(ops); } /** * \brief Advertise a topic, with full range of AdvertiseOptions * * This call connects to the master to publicize that the node will be * publishing messages on the given topic. This method returns a Publisher that allows you to * publish a message on this topic. * * This is an advanced version advertise() that exposes all options (through the AdvertiseOptions structure) * * \param ops Advertise options to use * \return On success, a Publisher that, when it goes out of scope, will automatically release a reference * on this advertisement. On failure, an empty Publisher which can be checked with: \verbatim ros::NodeHandle nodeHandle; ros::AdvertiseOptions ops; ... ros::publisher pub = nodeHandle.advertise(ops); if (pub) // Enter if publisher is valid { ... } \endverbatim * * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name */ Publisher advertise(AdvertiseOptions& ops); ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Versions of subscribe() ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * \brief Subscribe to a topic, version for class member function with bare pointer * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, fp is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe is a convenience function for using member functions, and can be used like so: \verbatim void Foo::callback(const std_msgs::Empty::ConstPtr& message) { } Foo foo_object; ros::Subscriber sub = handle.subscribe("my_topic", 1, &Foo::callback, &foo_object); \endverbatim * * \param M [template] M here is the callback parameter type (e.g. const boost::shared_ptr& or const M&), \b not the message type, and should almost always be deduced * \param topic Topic 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 fp Member function pointer to call when a message has arrived * \param obj Object to call fp on * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim ros::NodeHandle nodeHandle; void Foo::callback(const std_msgs::Empty::ConstPtr& message) {} boost::shared_ptr foo_object(new Foo); ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, &Foo::callback, foo_object); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template initByFullCallbackType(topic, queue_size, boost::bind(fp, obj, _1)); ops.transport_hints = transport_hints; return subscribe(ops); } /// and the const version template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M) const, T* obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template initByFullCallbackType(topic, queue_size, boost::bind(fp, obj, _1)); ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version for class member function with bare pointer * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, fp is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe is a convenience function for using member functions, and can be used like so: \verbatim void Foo::callback(const std_msgs::Empty::ConstPtr& message) { } Foo foo_object; ros::Subscriber sub = handle.subscribe("my_topic", 1, &Foo::callback, &foo_object); \endverbatim * * \param M [template] M here is the message type * \param topic Topic 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 fp Member function pointer to call when a message has arrived * \param obj Object to call fp on * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim ros::NodeHandle nodeHandle; void Foo::callback(const std_msgs::Empty::ConstPtr& message) {} boost::shared_ptr foo_object(new Foo); ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, &Foo::callback, foo_object); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr&), T* obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template init(topic, queue_size, boost::bind(fp, obj, _1)); ops.transport_hints = transport_hints; return subscribe(ops); } template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr&) const, T* obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template init(topic, queue_size, boost::bind(fp, obj, _1)); ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version for class member function with shared_ptr * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, fp is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe is a convenience function for using member functions on a shared_ptr: \verbatim void Foo::callback(const std_msgs::Empty::ConstPtr& message) { } boost::shared_ptr foo_object(new Foo); ros::Subscriber sub = handle.subscribe("my_topic", 1, &Foo::callback, foo_object); \endverbatim * * \param M [template] M here is the callback parameter type (e.g. const boost::shared_ptr& or const M&), \b not the message type, and should almost always be deduced * \param topic Topic 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 fp Member function pointer to call when a message has arrived * \param obj Object to call fp on. Since this is a shared pointer, the object will automatically be tracked with a weak_ptr * so that if it is deleted before the Subscriber goes out of scope the callback will no longer be called (and therefore will not crash). * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim ros::NodeHandle nodeHandle; void Foo::callback(const std_msgs::Empty::ConstPtr& message) {} boost::shared_ptr foo_object(new Foo); ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, &Foo::callback, foo_object); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M), const boost::shared_ptr& obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template initByFullCallbackType(topic, queue_size, boost::bind(fp, obj.get(), _1)); ops.tracked_object = obj; ops.transport_hints = transport_hints; return subscribe(ops); } template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M) const, const boost::shared_ptr& obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template initByFullCallbackType(topic, queue_size, boost::bind(fp, obj.get(), _1)); ops.tracked_object = obj; ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version for class member function with shared_ptr * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, fp is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe is a convenience function for using member functions on a shared_ptr: \verbatim void Foo::callback(const std_msgs::Empty::ConstPtr& message) { } boost::shared_ptr foo_object(new Foo); ros::Subscriber sub = handle.subscribe("my_topic", 1, &Foo::callback, foo_object); \endverbatim * * \param M [template] M here is the message type * \param topic Topic 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 fp Member function pointer to call when a message has arrived * \param obj Object to call fp on. Since this is a shared pointer, the object will automatically be tracked with a weak_ptr * so that if it is deleted before the Subscriber goes out of scope the callback will no longer be called (and therefore will not crash). * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim ros::NodeHandle nodeHandle; void Foo::callback(const std_msgs::Empty::ConstPtr& message) {} boost::shared_ptr foo_object(new Foo); ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, &Foo::callback, foo_object); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr&), const boost::shared_ptr& obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template init(topic, queue_size, boost::bind(fp, obj.get(), _1)); ops.tracked_object = obj; ops.transport_hints = transport_hints; return subscribe(ops); } template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(const boost::shared_ptr&) const, const boost::shared_ptr& obj, const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template init(topic, queue_size, boost::bind(fp, obj.get(), _1)); ops.tracked_object = obj; ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version for bare function * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, fp is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe is a convenience function for using bare functions, and can be used like so: \verbatim void callback(const std_msgs::Empty::ConstPtr& message) { } ros::Subscriber sub = handle.subscribe("my_topic", 1, callback); \endverbatim * * \param M [template] M here is the callback parameter type (e.g. const boost::shared_ptr& or const M&), \b not the message type, and should almost always be deduced * \param topic Topic 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 fp Function pointer to call when a message has arrived * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim void callback(const std_msgs::Empty::ConstPtr& message){...} ros::NodeHandle nodeHandle; ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, callback); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(*fp)(M), const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template initByFullCallbackType(topic, queue_size, fp); ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version for bare function * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, fp is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe is a convenience function for using bare functions, and can be used like so: \verbatim void callback(const std_msgs::Empty::ConstPtr& message) { } ros::Subscriber sub = handle.subscribe("my_topic", 1, callback); \endverbatim * * \param M [template] M here is the message type * \param topic Topic 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 fp Function pointer to call when a message has arrived * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim void callback(const std_msgs::Empty::ConstPtr& message){...} ros::NodeHandle nodeHandle; ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, callback); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(*fp)(const boost::shared_ptr&), const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template init(topic, queue_size, fp); ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version for arbitrary boost::function object * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, callback is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe allows anything bindable to a boost::function object * * \param M [template] M here is the message type * \param topic Topic 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 Callback to call when a message has arrived * \param tracked_object 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 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. * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim void callback(const std_msgs::Empty::ConstPtr& message){...} ros::NodeHandle nodeHandle; ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, callback); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, const boost::function&)>& callback, const VoidConstPtr& tracked_object = VoidConstPtr(), const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template init(topic, queue_size, callback); ops.tracked_object = tracked_object; ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version for arbitrary boost::function object * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, callback is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe allows anything bindable to a boost::function object * * \param M [template] the message type * \param C [template] the callback parameter type (e.g. const boost::shared_ptr& or const M&) * \param topic Topic 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 Callback to call when a message has arrived * \param tracked_object 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 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. * \param transport_hints a TransportHints structure which defines various transport-related options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim void callback(const std_msgs::Empty::ConstPtr& message){...} ros::NodeHandle nodeHandle; ros::Subscriber sub = nodeHandle.subscribe("my_topic", 1, callback); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ template Subscriber subscribe(const std::string& topic, uint32_t queue_size, const boost::function& callback, const VoidConstPtr& tracked_object = VoidConstPtr(), const TransportHints& transport_hints = TransportHints()) { SubscribeOptions ops; ops.template initByFullCallbackType(topic, queue_size, callback); ops.tracked_object = tracked_object; ops.transport_hints = transport_hints; return subscribe(ops); } /** * \brief Subscribe to a topic, version with full range of SubscribeOptions * * This method connects to the master to register interest in a given * topic. The node will automatically be connected with publishers on * this topic. On each message receipt, fp is invoked and passed a shared pointer * to the message received. This message should \b not be changed in place, as it * is shared with any other subscriptions to this topic. * * This version of subscribe allows the full range of options, exposed through the SubscribeOptions class * * \param ops Subscribe options * \return On success, a Subscriber that, when all copies of it go out of scope, will unsubscribe from this topic. * On failure, an empty Subscriber which can be checked with: \verbatim SubscribeOptions ops; ... ros::NodeHandle nodeHandle; ros::Subscriber sub = nodeHandle.subscribe(ops); if (sub) // Enter if subscriber is valid { ... } \endverbatim * \throws InvalidNameException If the topic name begins with a tilde, or is an otherwise invalid graph resource name * \throws ConflictingSubscriptionException If this node is already subscribed to the same topic with a different datatype */ Subscriber subscribe(SubscribeOptions& ops); ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Versions of advertiseService() ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * \brief Advertise a service, version for class member function with bare pointer * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This is a convenience function for using member functions, and can be used like so: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } Foo foo_object; ros::ServiceServer service = handle.advertiseService("my_service", &Foo::callback, &foo_object); \endverbatim * * \param service Service name to advertise on * \param srv_func Member function pointer to call when a message has arrived * \param obj Object to call srv_func on * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", &Foo::callback, &foo_object); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, bool(T::*srv_func)(MReq &, MRes &), T *obj) { AdvertiseServiceOptions ops; ops.template init(service, boost::bind(srv_func, obj, _1, _2)); return advertiseService(ops); } /** * \brief Advertise a service, version for class member function with bare pointer using ros::ServiceEvent as the callback parameter type * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This is a convenience function for using member functions, and can be used like so: \verbatim bool Foo::callback(ros::ServiceEvent& event) { return true; } Foo foo_object; ros::ServiceServer service = handle.advertiseService("my_service", &Foo::callback, &foo_object); \endverbatim * * \param service Service name to advertise on * \param srv_func Member function pointer to call when a message has arrived * \param obj Object to call srv_func on * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", &Foo::callback, &foo_object); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, bool(T::*srv_func)(ServiceEvent&), T *obj) { AdvertiseServiceOptions ops; ops.template initBySpecType >(service, boost::bind(srv_func, obj, _1)); return advertiseService(ops); } /** * \brief Advertise a service, version for class member function with shared_ptr * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This is a convenience function for using member functions on shared pointers, and can be used like so: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } boost::shared_ptr foo_object(new Foo); ros::ServiceServer service = handle.advertiseService("my_service", &Foo::callback, foo_object); \endverbatim * * \param service Service name to advertise on * \param srv_func Member function pointer to call when a message has arrived * \param obj Object to call srv_func on. Since this is a shared_ptr, it will automatically be tracked with a weak_ptr, * and if the object is deleted the service callback will stop being called (and therefore will not crash). * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", &Foo::callback, &foo_object); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, bool(T::*srv_func)(MReq &, MRes &), const boost::shared_ptr& obj) { AdvertiseServiceOptions ops; ops.template init(service, boost::bind(srv_func, obj.get(), _1, _2)); ops.tracked_object = obj; return advertiseService(ops); } /** * \brief Advertise a service, version for class member function with shared_ptr using ros::ServiceEvent as the callback parameter type * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This is a convenience function for using member functions on shared pointers, and can be used like so: \verbatim bool Foo::callback(ros::ServiceEvent& event) { return true; } boost::shared_ptr foo_object(new Foo); ros::ServiceServer service = handle.advertiseService("my_service", &Foo::callback, foo_object); \endverbatim * * \param service Service name to advertise on * \param srv_func Member function pointer to call when a message has arrived * \param obj Object to call srv_func on. Since this is a shared_ptr, it will automatically be tracked with a weak_ptr, * and if the object is deleted the service callback will stop being called (and therefore will not crash). * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", &Foo::callback, &foo_object); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, bool(T::*srv_func)(ServiceEvent&), const boost::shared_ptr& obj) { AdvertiseServiceOptions ops; ops.template initBySpecType >(service, boost::bind(srv_func, obj.get(), _1)); ops.tracked_object = obj; return advertiseService(ops); } /** * \brief Advertise a service, version for bare function * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This is a convenience function for using bare functions, and can be used like so: \verbatim bool callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::ServiceServer service = handle.advertiseService("my_service", callback); \endverbatim * * \param service Service name to advertise on * \param srv_func function pointer to call when a message has arrived * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", callback); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, bool(*srv_func)(MReq&, MRes&)) { AdvertiseServiceOptions ops; ops.template init(service, srv_func); return advertiseService(ops); } /** * \brief Advertise a service, version for bare function using ros::ServiceEvent as the callback parameter type * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This is a convenience function for using bare functions, and can be used like so: \verbatim bool callback(ros::ServiceEvent& event) { return true; } ros::ServiceServer service = handle.advertiseService("my_service", callback); \endverbatim * * \param service Service name to advertise on * \param srv_func function pointer to call when a message has arrived * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", callback); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, bool(*srv_func)(ServiceEvent&)) { AdvertiseServiceOptions ops; ops.template initBySpecType >(service, srv_func); return advertiseService(ops); } /** * \brief Advertise a service, version for arbitrary boost::function object * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This version of advertiseService allows non-class functions, as well as functor objects and boost::bind (along with anything * else boost::function supports). * * \param service Service name to advertise on * \param callback Callback to call when the service is called * \param tracked_object 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 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. * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", callback); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, const boost::function& callback, const VoidConstPtr& tracked_object = VoidConstPtr()) { AdvertiseServiceOptions ops; ops.template init(service, callback); ops.tracked_object = tracked_object; return advertiseService(ops); } /** * \brief Advertise a service, version for arbitrary boost::function object using ros::ServiceEvent as the callback parameter type * * Note that the template parameter S is the full event type, e.g. ros::ServiceEvent * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This version of advertiseService allows non-class functions, as well as functor objects and boost::bind (along with anything * else boost::function supports). * * \param service Service name to advertise on * \param callback Callback to call when the service is called * \param tracked_object 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 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. * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim bool Foo::callback(std_srvs::Empty& request, std_srvs::Empty& response) { return true; } ros::NodeHandle nodeHandle; Foo foo_object; ros::ServiceServer service = nodeHandle.advertiseService("my_service", callback); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceServer advertiseService(const std::string& service, const boost::function& callback, const VoidConstPtr& tracked_object = VoidConstPtr()) { AdvertiseServiceOptions ops; ops.template initBySpecType(service, callback); ops.tracked_object = tracked_object; return advertiseService(ops); } /** * \brief Advertise a service, with full range of AdvertiseServiceOptions * * This call connects to the master to publicize that the node will be * offering an RPC service with the given name. * * This version of advertiseService allows the full set of options, exposed through the AdvertiseServiceOptions class * * \param ops Advertise options * \return On success, a ServiceServer that, when all copies of it go out of scope, will unadvertise this service. * On failure, an empty ServiceServer which can be checked with: \verbatim AdvertiseServiceOptions ops; ... ros::NodeHandle nodeHandle; ros::ServiceServer service = nodeHandle.advertiseService(ops); if (service) // Enter if advertised service is valid { ... } \endverbatim * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ ServiceServer advertiseService(AdvertiseServiceOptions& ops); ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Versions of serviceClient() ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** @brief Create a client for a service, version templated on two message types * * 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 * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceClient serviceClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string()) { ServiceClientOptions ops; ops.template init(service_name, persistent, header_values); return serviceClient(ops); } /** @brief Create a client for a service, version templated on service type * * 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 * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ template ServiceClient serviceClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string()) { ServiceClientOptions ops; ops.template init(service_name, persistent, header_values); return serviceClient(ops); } /** @brief Create a client for a service, version with full range of ServiceClientOptions * * When the last handle reference of a persistent connection is cleared, the connection will automatically close. * * @param ops The options for this service client * \throws InvalidNameException If the service name begins with a tilde, or is an otherwise invalid graph resource name */ ServiceClient serviceClient(ServiceClientOptions& ops); ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Versions of createTimer() ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * \brief Create a timer which will call a callback at the specified rate. This variant takes * a class member function, and a bare pointer to the object to call the method on. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param r The rate at which to call the callback * \param callback The method to call * \param obj The object to call the method on * \param oneshot If true, this timer will only fire once * \param autostart If true (default), return timer that is already started */ template Timer createTimer(Rate r, Handler h, Obj o, bool oneshot = false, bool autostart = true) const { return createTimer(r.expectedCycleTime(), h, o, oneshot, autostart); } /** * \brief Create a timer which will call a callback at the specified rate. This variant takes * a class member function, and a bare pointer to the object to call the method on. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param period The period at which to call the callback * \param callback The method to call * \param obj The object to call the method on * \param oneshot If true, this timer will only fire once * \param autostart If true (default), return timer that is already started */ template Timer createTimer(Duration period, void(T::*callback)(const TimerEvent&) const, T* obj, bool oneshot = false, bool autostart = true) const { return createTimer(period, boost::bind(callback, obj, _1), oneshot, autostart); } /** * \brief Create a timer which will call a callback at the specified rate. This variant takes * a class member function, and a bare pointer to the object to call the method on. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param period The period at which to call the callback * \param callback The method to call * \param obj The object to call the method on * \param oneshot If true, this timer will only fire once * \param autostart If true (default), return timer that is already started */ template Timer createTimer(Duration period, void(T::*callback)(const TimerEvent&), T* obj, bool oneshot = false, bool autostart = true) const { return createTimer(period, boost::bind(callback, obj, _1), oneshot, autostart); } /** * \brief Create a timer which will call a callback at the specified rate. This variant takes * a class member function, and a shared pointer to the object to call the method on. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param period The period at which to call the callback * \param callback The method to call * \param obj The object to call the method on. Since this is a shared pointer, the object will * automatically be tracked with a weak_ptr so that if it is deleted before the Timer goes out of * scope the callback will no longer be called (and therefore will not crash). * \param oneshot If true, this timer will only fire once * \param autostart If true (default), return timer that is already started */ template Timer createTimer(Duration period, void(T::*callback)(const TimerEvent&), const boost::shared_ptr& obj, bool oneshot = false, bool autostart = true) const { TimerOptions ops(period, boost::bind(callback, obj.get(), _1), 0); ops.tracked_object = obj; ops.oneshot = oneshot; ops.autostart = autostart; return createTimer(ops); } /** * \brief Create a timer which will call a callback at the specified rate. This variant takes * anything that can be bound to a Boost.Function, including a bare function * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param period The period at which to call the callback * \param callback The function to call * \param oneshot If true, this timer will only fire once * \param autostart If true (default), return timer that is already started */ Timer createTimer(Duration period, const TimerCallback& callback, bool oneshot = false, bool autostart = true) const; /** * \brief Create a timer which will call a callback at the specified rate. This variant allows * the full range of TimerOptions. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param ops The options to use when creating the timer */ Timer createTimer(TimerOptions& ops) const; ////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Versions of createWallTimer() ////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** * \brief Create a timer which will call a callback at the specified rate, using wall time to determine * when to call the callback instead of ROS time. * This variant takes a class member function, and a bare pointer to the object to call the method on. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param period The period at which to call the callback * \param callback The method to call * \param obj The object to call the method on * \param oneshot If true, this timer will only fire once * \param autostart If true (default), return timer that is already started */ template WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent&), T* obj, bool oneshot = false, bool autostart = true) const { return createWallTimer(period, boost::bind(callback, obj, _1), oneshot, autostart); } /** * \brief Create a timer which will call a callback at the specified rate, using wall time to determine * when to call the callback instead of ROS time. This variant takes * a class member function, and a shared pointer to the object to call the method on. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param period The period at which to call the callback * \param callback The method to call * \param obj The object to call the method on. Since this is a shared pointer, the object will * automatically be tracked with a weak_ptr so that if it is deleted before the Timer goes out of * scope the callback will no longer be called (and therefore will not crash). * \param oneshot If true, this timer will only fire once */ template WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent&), const boost::shared_ptr& obj, bool oneshot = false, bool autostart = true) const { WallTimerOptions ops(period, boost::bind(callback, obj.get(), _1), 0); ops.tracked_object = obj; ops.oneshot = oneshot; ops.autostart = autostart; return createWallTimer(ops); } /** * \brief Create a timer which will call a callback at the specified rate, using wall time to determine * when to call the callback instead of ROS time. This variant takes * anything that can be bound to a Boost.Function, including a bare function * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param period The period at which to call the callback * \param callback The function to call * \param oneshot If true, this timer will only fire once */ WallTimer createWallTimer(WallDuration period, const WallTimerCallback& callback, bool oneshot = false, bool autostart = true) const; /** * \brief Create a timer which will call a callback at the specified rate, using wall time to determine * when to call the callback instead of ROS time. This variant allows * the full range of TimerOptions. * * When the Timer (and all copies of it) returned goes out of scope, the timer will automatically * be stopped, and the callback will no longer be called. * * \param ops The options to use when creating the timer */ WallTimer createWallTimer(WallTimerOptions& ops) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const XmlRpc::XmlRpcValue& v) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::string& s) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const char* s) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, double d) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, int i) const; /** \brief Set a boolean 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, bool b) const; /** \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 value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::vector& vec) const; /** \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 value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::vector& vec) const; /** \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 value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::vector& vec) const; /** \brief Set a int vector value on the parameter server. * * \param key The key to be used in the parameter server's dictionary * \param vec The value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::vector& vec) const; /** \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 value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::vector& vec) const; /** \brief Set a string vector value on the parameter server. * * \param key The key to be used in the parameter server's dictionary * \param map The value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::map& map) const; /** \brief Set a double vector value on the parameter server. * * \param key The key to be used in the parameter server's dictionary * \param map The value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::map& map) const; /** \brief Set a float vector value on the parameter server. * * \param key The key to be used in the parameter server's dictionary * \param map The value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::map& map) const; /** \brief Set a int vector value on the parameter server. * * \param key The key to be used in the parameter server's dictionary * \param map The value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::map& map) const; /** \brief Set a bool vector value on the parameter server. * * \param key The key to be used in the parameter server's dictionary * \param map The value to be inserted. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ void setParam(const std::string& key, const std::map& map) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::string& s) const; /** \brief Get a double value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, double& d) const; /** \brief Get a float value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, float& f) const; /** \brief Get an integer value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, int& i) const; /** \brief Get a boolean value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, bool& b) const; /** \brief Get an arbitrary XML/RPC value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, XmlRpc::XmlRpcValue& v) const; /** \brief Get a string vector value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::vector& vec) const; /** \brief Get a double vector value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::vector& vec) const; /** \brief Get a float vector value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::vector& vec) const; /** \brief Get an int vector value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::vector& vec) const; /** \brief Get a boolean vector value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::vector& vec) const; /** \brief Get a string map value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::map& map) const; /** \brief Get a double map value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::map& map) const; /** \brief Get a float map value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::map& map) const; /** \brief Get an int map value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::map& map) const; /** \brief Get a boolean map value from the parameter server. * * If you want to provide a default value in case the key does not exist use param(). * * \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParam(const std::string& key, std::map& map) const; /** \brief Get a string value from the parameter server, with local caching * * If you want to provide a default value in case the key does not exist use param(). * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::string& s) const; /** \brief Get a double value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, double& d) const; /** \brief Get a float value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, float& f) const; /** \brief Get an integer value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, int& i) const; /** \brief Get a boolean value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, bool& b) const; /** \brief Get an arbitrary XML/RPC value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, XmlRpc::XmlRpcValue& v) const; /** \brief Get a std::string vector value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::vector& vec) const; /** \brief Get a double vector value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::vector& vec) const; /** \brief Get a float vector value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::vector& vec) const; /** \brief Get a int vector value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::vector& vec) const; /** \brief Get a bool vector value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::vector& vec) const; /** \brief Get a string->std::string map value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::map& map) const; /** \brief Get a string->double map value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::map& map) const; /** \brief Get a string->float map value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::map& map) const; /** \brief Get a string->int map value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::map& map) const; /** \brief Get a string->bool map value from the parameter server, with local caching * * This method 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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool getParamCached(const std::string& key, std::map& map) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool hasParam(const std::string& key) const; /** \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're in 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. */ bool searchParam(const std::string& key, std::string& result) const; /** \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 parameter key begins with a tilde, or is an otherwise invalid graph resource name */ bool deleteParam(const std::string& key) const; /** \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. * \throws InvalidNameException If the parameter key begins with a tilde, or is an otherwise invalid graph resource name */ template void param(const std::string& param_name, T& param_val, const T& default_val) const { if (hasParam(param_name)) { if (getParam(param_name, param_val)) { return; } } param_val = default_val; } /** * \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 parameter key begins with a tilde, * or is an otherwise invalid graph resource name. */ template T param(const std::string& param_name, const T& default_val) { T param_val; param(param_name, param_val, default_val); return param_val; } /** * \brief Shutdown every handle created through this NodeHandle. * * This method will unadvertise every topic and service advertisement, * and unsubscribe every subscription created through this NodeHandle. */ void shutdown(); /** \brief Check whether it's time to exit. * * This method checks to see if both ros::ok() is true and shutdown() has not been called on this NodeHandle, to see whether it's yet time * to exit. ok() is false once either ros::shutdown() or NodeHandle::shutdown() have been called * * \return true if we're still OK, false if it's time to exit */ bool ok() const; private: struct no_validate { }; // this is pretty awful, but required to preserve public interface (and make minimum possible changes) std::string resolveName(const std::string& name, bool remap, no_validate) const; void construct(const std::string& ns, bool validate_name); void destruct(); void initRemappings(const M_string& remappings); std::string remapName(const std::string& name) const; std::string namespace_; std::string unresolved_namespace_; M_string remappings_; M_string unresolved_remappings_; CallbackQueueInterface* callback_queue_; NodeHandleBackingCollection* collection_; bool ok_; }; } #endif // ROSCPP_NODE_HANDLE_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/param.h000066400000000000000000000707311262037435000226760ustar00rootroot00000000000000/* * 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 "XmlRpcValue.h" #include #include 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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& 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 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. * \throws InvalidNameException if the key is not a valid graph resource name */ template void param(const std::string& param_name, T& param_val, const T& default_val) { if (has(param_name)) { if (get(param_name, param_val)) { return; } } param_val = default_val; } /** * \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 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/parameter_adapter.h000066400000000000000000000146711262037435000252570ustar00rootroot00000000000000/* * 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 #include #include #include 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&); void callback(const boost::shared_ptr&); void callback(boost::shared_ptr); void callback(boost::shared_ptr); void callback(const M&); void callback(M); void callback(const MessageEvent&); void callback(const MessageEvent&); \endverbatim */ template struct ParameterAdapter { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef M Parameter; static const bool is_const = true; static Parameter getParameter(const Event& event) { return *event.getMessage(); } }; template struct ParameterAdapter& > { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef const boost::shared_ptr Parameter; static const bool is_const = true; static Parameter getParameter(const Event& event) { return event.getMessage(); } }; template struct ParameterAdapter& > { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef boost::shared_ptr Parameter; static const bool is_const = false; static Parameter getParameter(const Event& event) { return ros::MessageEvent(event).getMessage(); } }; template struct ParameterAdapter { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef const M& Parameter; static const bool is_const = true; static Parameter getParameter(const Event& event) { return *event.getMessage(); } }; template struct ParameterAdapter > { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef boost::shared_ptr Parameter; static const bool is_const = true; static Parameter getParameter(const Event& event) { return event.getMessage(); } }; template struct ParameterAdapter > { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef boost::shared_ptr Parameter; static const bool is_const = false; static Parameter getParameter(const Event& event) { return ros::MessageEvent(event).getMessage(); } }; template struct ParameterAdapter& > { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef const ros::MessageEvent& Parameter; static const bool is_const = true; static Parameter getParameter(const Event& event) { return event; } }; template struct ParameterAdapter& > { typedef typename boost::remove_reference::type>::type Message; typedef ros::MessageEvent Event; typedef ros::MessageEvent Parameter; static const bool is_const = false; static Parameter getParameter(const Event& event) { return ros::MessageEvent(event); } }; } #endif // ROSCPP_PARAMETER_ADAPTER_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/poll_manager.h000066400000000000000000000047431262037435000242360ustar00rootroot00000000000000/* * 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 #include #include namespace ros { class PollManager; typedef boost::shared_ptr PollManagerPtr; typedef boost::signals2::signal VoidSignal; typedef boost::function 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/poll_set.h000066400000000000000000000112571262037435000234150ustar00rootroot00000000000000/* * 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 #include "io.h" #include "common.h" #include #include #include namespace ros { class Transport; typedef boost::shared_ptr 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 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 M_SocketInfo; M_SocketInfo socket_info_; boost::mutex socket_info_mutex_; bool sockets_changed_; boost::mutex just_deleted_mutex_; typedef std::vector V_int; V_int just_deleted_; std::vector ufds_; boost::mutex signal_mutex_; signal_fd_t signal_pipe_[2]; }; } #endif // ROSCPP_POLL_SET_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/publication.h000066400000000000000000000135361262037435000241070ustar00rootroot00000000000000/* * 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 "XmlRpc.h" #include #include #include #include namespace ros { class SubscriberLink; typedef boost::shared_ptr SubscriberLinkPtr; typedef std::vector 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 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 V_SerializedMessage; V_SerializedMessage publish_queue_; boost::mutex publish_queue_mutex_; }; } #endif // ROSCPP_PUBLICATION_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/publisher.h000066400000000000000000000155011262037435000235650ustar00rootroot00000000000000/* * 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 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 void publish(const boost::shared_ptr& 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(*message)) == "*" || impl_->md5sum_ == mt::md5sum(*message), "Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]", mt::datatype(*message), mt::md5sum(*message), impl_->datatype_.c_str(), impl_->md5sum_.c_str()); SerializedMessage m; m.type_info = &typeid(M); m.message = message; publish(boost::bind(serializeMessage, boost::ref(*message)), m); } /** * \brief Publish a message on the topic associated with this Publisher. */ template 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(message)) == "*" || impl_->md5sum_ == mt::md5sum(message), "Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]", mt::datatype(message), mt::md5sum(message), impl_->datatype_.c_str(), impl_->md5sum_.c_str()); SerializedMessage m; publish(boost::bind(serializeMessage, 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& 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 ImplPtr; typedef boost::weak_ptr ImplWPtr; ImplPtr impl_; friend class NodeHandle; friend class NodeHandleBackingCollection; }; typedef std::vector V_Publisher; } #endif // ROSCPP_PUBLISHER_HANDLE_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/publisher_link.h000066400000000000000000000072161262037435000246060ustar00rootroot00000000000000/* * 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 #include #include #include #include namespace ros { class Header; class Message; class Subscription; typedef boost::shared_ptr SubscriptionPtr; typedef boost::weak_ptr SubscriptionWPtr; class Connection; typedef boost::shared_ptr 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 { 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/ros.h000066400000000000000000000042151262037435000223730ustar00rootroot00000000000000/* * 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/roscpp.dox000066400000000000000000000017431262037435000234440ustar00rootroot00000000000000/** @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 on the wiki and in the roscpp_tutorials package. */ ros-ros-comm-1.11.16/clients/roscpp/include/ros/rosout_appender.h000066400000000000000000000051431262037435000250020ustar00rootroot00000000000000/* * 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 #include "common.h" #include #include #include namespace rosgraph_msgs { ROS_DECLARE_MESSAGE(Log); } namespace ros { class Publication; typedef boost::shared_ptr PublicationPtr; typedef boost::weak_ptr 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 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/service.h000066400000000000000000000150101262037435000232230ustar00rootroot00000000000000/* * 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 #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 namespace ros { class ServiceServerLink; typedef boost::shared_ptr 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 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 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 ServiceClient createClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string()) { NodeHandle nh; ServiceClient client = nh.template serviceClient(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 ServiceClient createClient(const std::string& service_name, bool persistent = false, const M_string& header_values = M_string()) { NodeHandle nh; ServiceClient client = nh.template serviceClient(ros::names::resolve(service_name), persistent, header_values); return client; } } } #endif // ROSCPP_SERVICE_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_callback_helper.h000066400000000000000000000155421262037435000264100ustar00rootroot00000000000000/* * 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 #include namespace ros { struct ROSCPP_DECL ServiceCallbackHelperCallParams { SerializedMessage request; SerializedMessage response; boost::shared_ptr connection_header; }; template inline boost::shared_ptr defaultServiceCreateFunction() { return boost::shared_ptr(new M); } template struct ServiceSpecCallParams { boost::shared_ptr request; boost::shared_ptr response; boost::shared_ptr connection_header; }; /** * \brief Event type for services, ros::ServiceEvent& 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 class ServiceEvent { public: typedef MReq RequestType; typedef MRes ResponseType; typedef boost::shared_ptr RequestPtr; typedef boost::shared_ptr ResponsePtr; typedef boost::function&)> CallbackType; static bool call(const CallbackType& cb, ServiceSpecCallParams& params) { ServiceEvent event(params.request, params.response, params.connection_header); return cb(event); } ServiceEvent(const boost::shared_ptr& req, const boost::shared_ptr& res, const boost::shared_ptr& 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 request_; boost::shared_ptr response_; boost::shared_ptr connection_header_; }; template struct ServiceSpec { typedef MReq RequestType; typedef MRes ResponseType; typedef boost::shared_ptr RequestPtr; typedef boost::shared_ptr ResponsePtr; typedef boost::function CallbackType; static bool call(const CallbackType& cb, ServiceSpecCallParams& 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 ServiceCallbackHelperPtr; /** * \brief Concrete generic implementation of ServiceCallbackHelper for any normal service type */ template 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 ReqCreateFunction; typedef boost::function 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(defaultServiceCreateFunction), const ResCreateFunction& create_res = static_cast(defaultServiceCreateFunction)) : 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 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_client.h000066400000000000000000000151571262037435000245750ustar00rootroot00000000000000/* * 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 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 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 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 ImplPtr; typedef boost::weak_ptr ImplWPtr; ImplPtr impl_; friend class NodeHandle; friend class NodeHandleBackingCollection; }; typedef boost::shared_ptr ServiceClientPtr; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_client_link.h000066400000000000000000000066371262037435000256150ustar00rootroot00000000000000/* * 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 #include #include #include #include namespace ros { class Header; class ServicePublication; typedef boost::weak_ptr ServicePublicationWPtr; typedef boost::shared_ptr ServicePublicationPtr; class Connection; typedef boost::shared_ptr ConnectionPtr; /** * \brief Handles a connection to a single incoming service client. */ class ROSCPP_DECL ServiceClientLink : public boost::enable_shared_from_this { 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& buffer, uint32_t size, bool success); void onRequest(const ConnectionPtr& conn, const boost::shared_array& 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 ServiceClientLinkPtr; } // namespace ros #endif // ROSCPP_PUBLISHER_DATA_HANDLER_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_client_options.h000066400000000000000000000103541262037435000263420ustar00rootroot00000000000000/* * 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 void init(const std::string& _service, bool _persistent, const M_string& _header) { namespace st = service_traits; service = _service; md5sum = st::md5sum(); 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 void init(const std::string& _service, bool _persistent, const M_string& _header) { namespace st = service_traits; service = _service; md5sum = st::md5sum(); 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_manager.h000066400000000000000000000123771262037435000247320ustar00rootroot00000000000000/* * 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 #include namespace ros { class ServiceManager; typedef boost::shared_ptr ServiceManagerPtr; class PollManager; typedef boost::shared_ptr PollManagerPtr; class XMLRPCManager; typedef boost::shared_ptr XMLRPCManagerPtr; class ConnectionManager; typedef boost::shared_ptr 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_publication.h000066400000000000000000000105471262037435000256260ustar00rootroot00000000000000/* * 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 "XmlRpc.h" #include #include #include #include #include #include #include namespace ros { class ServiceClientLink; typedef boost::shared_ptr ServiceClientLinkPtr; typedef std::vector 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 { 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 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 ServicePublicationPtr; } #endif // ROSCPP_SERVICE_PUBLICATION_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_server.h000066400000000000000000000067401262037435000246230ustar00rootroot00000000000000/* * 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 ImplPtr; typedef boost::weak_ptr ImplWPtr; ImplPtr impl_; friend class NodeHandle; friend class NodeHandleBackingCollection; }; typedef std::vector V_ServiceServer; } #endif // ROSCPP_SERVICE_HANDLE_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/service_server_link.h000066400000000000000000000125341262037435000256360ustar00rootroot00000000000000/* * 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 #include #include #include #include namespace ros { class Header; class Message; class Connection; typedef boost::shared_ptr 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 { 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 CallInfoPtr; typedef std::queue Q_CallInfo; public: typedef std::map 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& buffer, uint32_t size, bool success); void onResponse(const ConnectionPtr& conn, const boost::shared_array& 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 ServiceServerLinkPtr; } // namespace ros #endif // ROSCPP_SERVICE_SERVER_LINK_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/single_subscriber_publisher.h000066400000000000000000000067751262037435000273660ustar00rootroot00000000000000/* * 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 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 void publish(const boost::shared_ptr& 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 void publish(const boost::shared_ptr& message) const { publish(*message); } /** * \brief Publish a message on the topic associated with this Publisher. */ template 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/spinner.h000066400000000000000000000101421262037435000232420ustar00rootroot00000000000000/* * 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 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 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. A spinner can't be started if * another spinner is already running. */ 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/statistics.h000066400000000000000000000070151262037435000237630ustar00rootroot00000000000000/* * 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 #include "ros/subscription_callback_helper.h" #include 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& 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 arrival_time_list; // age of all messages within the current window (if available) std::list 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 map_; }; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/subscribe_options.h000066400000000000000000000170501262037435000253250ustar00rootroot00000000000000/* * 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 void initByFullCallbackType(const std::string& _topic, uint32_t _queue_size, const boost::function& _callback, const boost::function::Message>(void)>& factory_fn = DefaultMessageCreator::Message>()) { typedef typename ParameterAdapter

::Message MessageType; topic = _topic; queue_size = _queue_size; md5sum = message_traits::md5sum(); datatype = message_traits::datatype(); helper = SubscriptionCallbackHelperPtr(new SubscriptionCallbackHelperT

(_callback, factory_fn)); } /** * \brief Templated initialization, templated on message type. Only supports "const boost::shared_ptr&" 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 void init(const std::string& _topic, uint32_t _queue_size, const boost::function&)>& _callback, const boost::function(void)>& factory_fn = DefaultMessageCreator()) { typedef typename ParameterAdapter::Message MessageType; topic = _topic; queue_size = _queue_size; md5sum = message_traits::md5sum(); datatype = message_traits::datatype(); helper = SubscriptionCallbackHelperPtr(new SubscriptionCallbackHelperT&>(_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 static SubscribeOptions create(const std::string& topic, uint32_t queue_size, const boost::function&)>& callback, const VoidConstPtr& tracked_object, CallbackQueueInterface* queue) { SubscribeOptions ops; ops.init(topic, queue_size, callback); ops.tracked_object = tracked_object; ops.callback_queue = queue; return ops; } }; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/subscriber.h000066400000000000000000000074151262037435000237400ustar00rootroot00000000000000/* * 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 ImplPtr; typedef boost::weak_ptr ImplWPtr; ImplPtr impl_; friend class NodeHandle; friend class NodeHandleBackingCollection; }; typedef std::vector V_Subscriber; } #endif // ROSCPP_PUBLISHER_HANDLE_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/subscriber_link.h000066400000000000000000000067511262037435000247570ustar00rootroot00000000000000/* * 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 #include #include #include #include namespace ros { class Header; class Message; class Publication; typedef boost::shared_ptr PublicationPtr; typedef boost::weak_ptr PublicationWPtr; class Connection; typedef boost::shared_ptr ConnectionPtr; class ROSCPP_DECL SubscriberLink : public boost::enable_shared_from_this { 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 nocopy, bool ser) = 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) { 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/subscription.h000066400000000000000000000202551262037435000243160ustar00rootroot00000000000000/* * 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 #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 "XmlRpc.h" #include #include #include namespace ros { class PublisherLink; typedef boost::shared_ptr PublisherLinkPtr; class SubscriptionCallback; typedef boost::shared_ptr SubscriptionCallbackPtr; class SubscriptionQueue; typedef boost::shared_ptr SubscriptionQueuePtr; class MessageDeserializer; typedef boost::shared_ptr MessageDeserializerPtr; class SubscriptionCallbackHelper; typedef boost::shared_ptr SubscriptionCallbackHelperPtr; /** * \brief Manages a subscription on a single topic. */ class ROSCPP_DECL Subscription : public boost::enable_shared_from_this { 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 &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 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& 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(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 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 CallbackInfoPtr; typedef std::vector 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 S_PendingConnection; S_PendingConnection pending_connections_; boost::mutex pending_connections_mutex_; typedef std::vector 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 > connection_header; ros::Time receipt_time; }; typedef std::map M_PublisherLinkToLatchInfo; M_PublisherLinkToLatchInfo latched_messages_; typedef std::vector > V_TypeAndDeserializer; V_TypeAndDeserializer cached_deserializers_; }; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/subscription_callback_helper.h000066400000000000000000000125701262037435000274720ustar00rootroot00000000000000/* * 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 #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 #include #include #include #include #include #include namespace ros { struct SubscriptionCallbackHelperDeserializeParams { uint8_t* buffer; uint32_t length; boost::shared_ptr connection_header; }; struct ROSCPP_DECL SubscriptionCallbackHelperCallParams { MessageEvent 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 SubscriptionCallbackHelperPtr; /** * \brief Concrete generic implementation of * SubscriptionCallbackHelper for any normal message type. Use * directly with care, this is mostly for internal use. */ template class SubscriptionCallbackHelperT : public SubscriptionCallbackHelper { public: typedef ParameterAdapter

Adapter; typedef typename ParameterAdapter

::Message NonConstType; typedef typename ParameterAdapter

::Event Event; typedef typename boost::add_const::type ConstType; typedef boost::shared_ptr NonConstTypePtr; typedef boost::shared_ptr ConstTypePtr; static const bool is_const = ParameterAdapter

::is_const; typedef boost::function Callback; typedef boost::function CreateFunction; SubscriptionCallbackHelperT(const Callback& callback, const CreateFunction& create = DefaultMessageCreator()) : callback_(callback) , create_(create) { } void setCreateFunction(const CreateFunction& create) { create_ = create; } virtual bool hasHeader() { return message_traits::hasHeader::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 predes_params; predes_params.message = msg; predes_params.connection_header = params.connection_header; ser::PreDeserialize::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

::getParameter(event)); } virtual const std::type_info& getTypeInfo() { return typeid(NonConstType); } virtual bool isConst() { return ParameterAdapter

::is_const; } private: Callback callback_; CreateFunction create_; }; } #endif // ROSCPP_SUBSCRIPTION_CALLBACK_HELPER_H ros-ros-comm-1.11.16/clients/roscpp/include/ros/subscription_queue.h000066400000000000000000000063431262037435000255240ustar00rootroot00000000000000/* * 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 #include #include #include namespace ros { class MessageDeserializer; typedef boost::shared_ptr MessageDeserializerPtr; class SubscriptionCallbackHelper; typedef boost::shared_ptr SubscriptionCallbackHelperPtr; class ROSCPP_DECL SubscriptionQueue : public CallbackInterface, public boost::enable_shared_from_this { 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 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/this_node.h000066400000000000000000000045771262037435000235570ustar00rootroot00000000000000/* * 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/timer.h000066400000000000000000000070721262037435000227140ustar00rootroot00000000000000/* * 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 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 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 ImplPtr; typedef boost::weak_ptr ImplWPtr; ImplPtr impl_; friend class NodeHandle; }; } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/timer_manager.h000066400000000000000000000351721262037435000244100ustar00rootroot00000000000000/* * 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 #include "ros/forwards.h" #include "ros/time.h" #include "ros/file_log.h" #include #include #include #include #include "ros/assert.h" #include "ros/callback_queue_interface.h" #include #include namespace ros { template class TimerManager { private: struct TimerInfo { int32_t handle; D period; boost::function 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 TimerInfoPtr; typedef boost::weak_ptr TimerInfoWPtr; typedef std::vector V_TimerInfo; typedef std::list L_int32; public: TimerManager(); ~TimerManager(); int32_t add(const D& period, const boost::function& 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 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* 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; WallTime cb_start = WallTime::now(); info->callback(event); WallTime cb_end = WallTime::now(); info->last_cb_duration = cb_end - cb_start; info->last_real = event.current_real; parent_->schedule(info); } return Success; } private: TimerManager* parent_; TimerInfoWPtr info_; T last_expected_; T last_real_; T current_expected_; bool called_; }; }; template TimerManager::TimerManager() : new_timer_(false), id_counter_(0), thread_started_(false), quit_(false) { } template TimerManager::~TimerManager() { quit_ = true; { boost::mutex::scoped_lock lock(timers_mutex_); timers_cond_.notify_all(); } if (thread_started_) { thread_.join(); } } template bool TimerManager::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 typename TimerManager::TimerInfoPtr TimerManager::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 bool TimerManager::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 int32_t TimerManager::add(const D& period, const boost::function& callback, CallbackQueueInterface* callback_queue, const VoidConstPtr& tracked_object, bool oneshot) { TimerInfoPtr info(new 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 void TimerManager::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 void TimerManager::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 void TimerManager::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 void TimerManager::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 void TimerManager::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(new 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 int32_t remaining_time = std::max((int32_t)((sleep_end - current).toSec() * 1000.0f), 1); timers_cond_.timed_wait(lock, boost::posix_time::milliseconds(remaining_time)); } } new_timer_ = false; } } } #endif ros-ros-comm-1.11.16/clients/roscpp/include/ros/timer_options.h000066400000000000000000000061611262037435000244650ustar00rootroot00000000000000/* * 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 ros-ros-comm-1.11.16/clients/roscpp/include/ros/topic.h000066400000000000000000000111261262037435000227050ustar00rootroot00000000000000/* * 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 namespace ros { namespace topic { /** * \brief Internal method, do not use */ ROSCPP_DECL void waitForMessageImpl(SubscribeOptions& ops, const boost::function& ready_pred, NodeHandle& nh, ros::Duration timeout); template class SubscribeHelper { public: typedef boost::shared_ptr MConstPtr; void callback(const MConstPtr& message) { message_ = message; } bool hasMessage() { return static_cast(message_); } MConstPtr getMessage() { return message_; } private: MConstPtr message_; }; /** * \brief Wait for a single message to arrive on a topic, with timeout * * \param M