add MonotonicTimer

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

clients/roscpp/include/ros/forwards.h

@@ -161,6 +161,24 @@ struct WallTimerEvent
 };
 typedef boost::function<void(const WallTimerEvent&)> 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<void(const MonotonicTimerEvent&)> MonotonicTimerCallback;
+
 class ServiceManager;
 typedef boost::shared_ptr<ServiceManager> ServiceManagerPtr;
 class TopicManager;

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<Impl> ImplPtr;
+  typedef boost::weak_ptr<Impl> ImplWPtr;
+
+  ImplPtr impl_;
+
+  friend class NodeHandle;
+};
+
+}
+
+#endif

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
+

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<class T>
+  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<class T>
+  MonotonicTimer createMonotonicTimer(WallDuration period, void(T::*callback)(const MonotonicTimerEvent&),
+                                      const boost::shared_ptr<T>& 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