From baa3dc44fd2ff2046aa5f93e4638334bebe5c196 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 26 Feb 2017 16:00:55 +0100 Subject: [PATCH] add MonotonicTimer based on MonotonicTime (which uses the CLOCK_MONOTONIC). This timer is not influenced by time jumps of the system time, so ideal for things like periodic checks of timeout/heartbeat, etc... --- clients/roscpp/CMakeLists.txt | 1 + clients/roscpp/include/ros/forwards.h | 18 +++ clients/roscpp/include/ros/monotonic_timer.h | 127 +++++++++++++++ .../include/ros/monotonic_timer_options.h | 86 +++++++++++ clients/roscpp/include/ros/node_handle.h | 80 ++++++++++ clients/roscpp/src/libros/monotonic_timer.cpp | 145 ++++++++++++++++++ clients/roscpp/src/libros/node_handle.cpp | 39 ++++- 7 files changed, 492 insertions(+), 4 deletions(-) create mode 100644 clients/roscpp/include/ros/monotonic_timer.h create mode 100644 clients/roscpp/include/ros/monotonic_timer_options.h create mode 100644 clients/roscpp/src/libros/monotonic_timer.cpp diff --git a/clients/roscpp/CMakeLists.txt b/clients/roscpp/CMakeLists.txt index 633102919e..f34af37f45 100644 --- a/clients/roscpp/CMakeLists.txt +++ b/clients/roscpp/CMakeLists.txt @@ -118,6 +118,7 @@ add_library(roscpp src/libros/poll_set.cpp src/libros/service.cpp src/libros/this_node.cpp + src/libros/monotonic_timer.cpp ) add_dependencies(roscpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/clients/roscpp/include/ros/forwards.h b/clients/roscpp/include/ros/forwards.h index 9095ce55e2..465a93318e 100644 --- a/clients/roscpp/include/ros/forwards.h +++ b/clients/roscpp/include/ros/forwards.h @@ -161,6 +161,24 @@ struct WallTimerEvent }; typedef boost::function WallTimerCallback; +/** + * \brief Structure passed as a parameter to the callback invoked by a ros::MonotonicTimer + */ +struct MonotonicTimerEvent +{ + MonotonicTime last_expected; ///< In a perfect world, this is when the last callback should have happened + MonotonicTime last_real; ///< When the last callback actually happened + + MonotonicTime current_expected; ///< In a perfect world, this is when the current callback should be happening + MonotonicTime 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 MonotonicTimerCallback; + class ServiceManager; typedef boost::shared_ptr ServiceManagerPtr; class TopicManager; diff --git a/clients/roscpp/include/ros/monotonic_timer.h b/clients/roscpp/include/ros/monotonic_timer.h new file mode 100644 index 0000000000..dad0ca0450 --- /dev/null +++ b/clients/roscpp/include/ros/monotonic_timer.h @@ -0,0 +1,127 @@ +/* + * 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_MONOTONIC_TIMER_H +#define ROSCPP_MONOTONIC_TIMER_H + +#include "common.h" +#include "forwards.h" +#include "monotonic_timer_options.h" + +namespace ros +{ + +/** + * \brief Manages a wall-clock timer callback + * + * A MonotonicTimer should always be created through a call to NodeHandle::createMonotonicTimer(), or copied from one + * that was. Once all copies of a specific + * MonotonicTimer go out of scope, the callback associated with that handle will stop + * being called. + */ +class ROSCPP_DECL MonotonicTimer +{ +public: + MonotonicTimer() {} + MonotonicTimer(const MonotonicTimer& rhs); + ~MonotonicTimer(); + + /** + * \brief Start the timer. Does nothing if the timer is already started. + */ + void start(); + /** + * \brief Stop the timer. Once this call returns, no more callbacks will be called. Does + * nothing if the timer is already stopped. + */ + void stop(); + + /** + * \brief Returns whether or not the timer has any pending events to call. + */ + bool hasPending(); + + /** + * \brief Set the period of this timer + */ + void setPeriod(const WallDuration& period, bool reset=true); + + bool isValid() { return impl_ && impl_->isValid(); } + operator void*() { return isValid() ? (void *) 1 : (void *) 0; } + + bool operator<(const MonotonicTimer& rhs) + { + return impl_ < rhs.impl_; + } + + bool operator==(const MonotonicTimer& rhs) + { + return impl_ == rhs.impl_; + } + + bool operator!=(const MonotonicTimer& rhs) + { + return impl_ != rhs.impl_; + } + +private: + MonotonicTimer(const MonotonicTimerOptions& ops); + + class Impl + { + public: + Impl(); + ~Impl(); + + bool isValid(); + bool hasPending(); + void setPeriod(const WallDuration &period, bool reset=true); + + void start(); + void stop(); + + bool started_; + int32_t timer_handle_; + + WallDuration period_; + MonotonicTimerCallback 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 diff --git a/clients/roscpp/include/ros/monotonic_timer_options.h b/clients/roscpp/include/ros/monotonic_timer_options.h new file mode 100644 index 0000000000..c15ed9b75c --- /dev/null +++ b/clients/roscpp/include/ros/monotonic_timer_options.h @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2009, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSCPP_MONOTONIC_TIMER_OPTIONS_H +#define ROSCPP_MONOTONIC_TIMER_OPTIONS_H + +#include "common.h" +#include "ros/forwards.h" + +namespace ros +{ + +/** + * \brief Encapsulates all options available for starting a timer + */ +struct ROSCPP_DECL MonotonicTimerOptions +{ + MonotonicTimerOptions() + : period(0.1) + , callback_queue(0) + , oneshot(false) + , autostart(true) + { + } + + /* + * \brief Constructor + * \param + */ + MonotonicTimerOptions(WallDuration _period, const MonotonicTimerCallback& _callback, CallbackQueueInterface* _queue, + bool oneshot = false, bool autostart = true) + : period(_period) + , callback(_callback) + , callback_queue(_queue) + , oneshot(oneshot) + , autostart(autostart) + {} + + WallDuration period; ///< The period to call the callback at + MonotonicTimerCallback 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 + diff --git a/clients/roscpp/include/ros/node_handle.h b/clients/roscpp/include/ros/node_handle.h index 638011458b..49a5fed626 100644 --- a/clients/roscpp/include/ros/node_handle.h +++ b/clients/roscpp/include/ros/node_handle.h @@ -36,6 +36,7 @@ #include "ros/timer.h" #include "ros/rate.h" #include "ros/wall_timer.h" +#include "ros/monotonic_timer.h" #include "ros/advertise_options.h" #include "ros/advertise_service_options.h" #include "ros/subscribe_options.h" @@ -1466,6 +1467,85 @@ if (service) // Enter if advertised service is valid */ WallTimer createWallTimer(WallTimerOptions& ops) const; + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Versions of createMonotonicTimer() + ////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + /** + * \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 + MonotonicTimer createMonotonicTimer(WallDuration period, void(T::*callback)(const MonotonicTimerEvent&), T* obj, + bool oneshot = false, bool autostart = true) const + { + return createMonotonicTimer(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 + MonotonicTimer createMonotonicTimer(WallDuration period, void(T::*callback)(const MonotonicTimerEvent&), + const boost::shared_ptr& obj, + bool oneshot = false, bool autostart = true) const + { + MonotonicTimerOptions ops(period, boost::bind(callback, obj.get(), _1), 0); + ops.tracked_object = obj; + ops.oneshot = oneshot; + ops.autostart = autostart; + return createMonotonicTimer(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 + */ + MonotonicTimer createMonotonicTimer(WallDuration period, const MonotonicTimerCallback& 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 + */ + MonotonicTimer createMonotonicTimer(MonotonicTimerOptions& 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 diff --git a/clients/roscpp/src/libros/monotonic_timer.cpp b/clients/roscpp/src/libros/monotonic_timer.cpp new file mode 100644 index 0000000000..dd375e5cc6 --- /dev/null +++ b/clients/roscpp/src/libros/monotonic_timer.cpp @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2009, Willow Garage, Inc. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "ros/monotonic_timer.h" +#include "ros/timer_manager.h" + +namespace ros +{ + +MonotonicTimer::Impl::Impl() + : started_(false) + , timer_handle_(-1) +{ } + +MonotonicTimer::Impl::~Impl() +{ + ROS_DEBUG("MonotonicTimer deregistering callbacks."); + stop(); +} + +void MonotonicTimer::Impl::start() +{ + if (!started_) + { + VoidConstPtr tracked_object; + if (has_tracked_object_) + { + tracked_object = tracked_object_.lock(); + } + timer_handle_ = TimerManager::global().add(period_, callback_, callback_queue_, tracked_object, oneshot_); + started_ = true; + } +} + +void MonotonicTimer::Impl::stop() +{ + if (started_) + { + started_ = false; + TimerManager::global().remove(timer_handle_); + timer_handle_ = -1; + } +} + +bool MonotonicTimer::Impl::isValid() +{ + return !period_.isZero(); +} + +bool MonotonicTimer::Impl::hasPending() +{ + if (!isValid() || timer_handle_ == -1) + { + return false; + } + + return TimerManager::global().hasPending(timer_handle_); +} + +void MonotonicTimer::Impl::setPeriod(const WallDuration& period, bool reset) +{ + period_ = period; + TimerManager::global().setPeriod(timer_handle_, period, reset); +} + + +MonotonicTimer::MonotonicTimer(const MonotonicTimerOptions& ops) +: impl_(new Impl) +{ + impl_->period_ = ops.period; + impl_->callback_ = ops.callback; + impl_->callback_queue_ = ops.callback_queue; + impl_->tracked_object_ = ops.tracked_object; + impl_->has_tracked_object_ = (ops.tracked_object != NULL); + impl_->oneshot_ = ops.oneshot; +} + +MonotonicTimer::MonotonicTimer(const MonotonicTimer& rhs) +{ + impl_ = rhs.impl_; +} + +MonotonicTimer::~MonotonicTimer() +{ +} + +void MonotonicTimer::start() +{ + if (impl_) + { + impl_->start(); + } +} + +void MonotonicTimer::stop() +{ + if (impl_) + { + impl_->stop(); + } +} + +bool MonotonicTimer::hasPending() +{ + if (impl_) + { + return impl_->hasPending(); + } + + return false; +} + +void MonotonicTimer::setPeriod(const WallDuration& period, bool reset) +{ + if (impl_) + { + impl_->setPeriod(period, reset); + } +} + +} diff --git a/clients/roscpp/src/libros/node_handle.cpp b/clients/roscpp/src/libros/node_handle.cpp index 7dc079749c..6962965eac 100644 --- a/clients/roscpp/src/libros/node_handle.cpp +++ b/clients/roscpp/src/libros/node_handle.cpp @@ -117,7 +117,7 @@ NodeHandle::NodeHandle(const NodeHandle& rhs) remappings_ = rhs.remappings_; unresolved_remappings_ = rhs.unresolved_remappings_; - construct(rhs.namespace_, true); + construct(rhs.namespace_, true); unresolved_namespace_ = rhs.unresolved_namespace_; } @@ -295,7 +295,7 @@ Publisher NodeHandle::advertise(AdvertiseOptions& ops) } } - SubscriberCallbacksPtr callbacks(boost::make_shared(ops.connect_cb, ops.disconnect_cb, + SubscriberCallbacksPtr callbacks(boost::make_shared(ops.connect_cb, ops.disconnect_cb, ops.tracked_object, ops.callback_queue)); if (TopicManager::instance()->advertise(ops, callbacks)) @@ -387,7 +387,7 @@ ServiceClient NodeHandle::serviceClient(ServiceClientOptions& ops) return client; } -Timer NodeHandle::createTimer(Duration period, const TimerCallback& callback, +Timer NodeHandle::createTimer(Duration period, const TimerCallback& callback, bool oneshot, bool autostart) const { TimerOptions ops; @@ -418,7 +418,7 @@ Timer NodeHandle::createTimer(TimerOptions& ops) const return timer; } -WallTimer NodeHandle::createWallTimer(WallDuration period, const WallTimerCallback& callback, +WallTimer NodeHandle::createWallTimer(WallDuration period, const WallTimerCallback& callback, bool oneshot, bool autostart) const { WallTimerOptions ops; @@ -449,6 +449,37 @@ WallTimer NodeHandle::createWallTimer(WallTimerOptions& ops) const return timer; } +MonotonicTimer NodeHandle::createMonotonicTimer(WallDuration period, const MonotonicTimerCallback& callback, + bool oneshot, bool autostart) const +{ + MonotonicTimerOptions ops; + ops.period = period; + ops.callback = callback; + ops.oneshot = oneshot; + ops.autostart = autostart; + return createMonotonicTimer(ops); +} + +MonotonicTimer NodeHandle::createMonotonicTimer(MonotonicTimerOptions& ops) const +{ + if (ops.callback_queue == 0) + { + if (callback_queue_) + { + ops.callback_queue = callback_queue_; + } + else + { + ops.callback_queue = getGlobalCallbackQueue(); + } + } + + MonotonicTimer timer(ops); + if (ops.autostart) + timer.start(); + return timer; +} + void NodeHandle::shutdown() { {