isolated_async_timer.h

Go to the documentation of this file.

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, University of Luxembourg
 *  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 University of Luxembourg 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.
 *
 * Author: Maciej Zurad
 *********************************************************************/
#ifndef ROBOT_ACTIVITY_ISOLATED_ASYNC_TIMER_H
#define ROBOT_ACTIVITY_ISOLATED_ASYNC_TIMER_H

#include <atomic>

#include <ros/ros.h>
#include <ros/console.h>
#include <ros/callback_queue.h>

namespace robot_activity
{

class IsolatedAsyncTimer
{
public:
  typedef std::function<void(void)> LambdaCallback;

  IsolatedAsyncTimer() = delete;

  IsolatedAsyncTimer(const ros::NodeHandle& node_handle,
                     const IsolatedAsyncTimer::LambdaCallback& callback,
                     const float& frequency,
                     bool stoppable = true,
                     bool autostart = true,
                     bool oneshot = false)
    : IsolatedAsyncTimer(node_handle,
                         to_timer_callback(callback),
                         frequency,
                         stoppable,
                         autostart,
                         oneshot) { }

  IsolatedAsyncTimer(const ros::NodeHandle& node_handle,
                     const ros::TimerCallback& callback,
                     const float& frequency,
                     bool stoppable = true,
                     bool autostart = true,
                     bool oneshot = false)
    : node_handle_(node_handle),
      timer_ops_(),
      callback_queue_(),
      stoppable_(stoppable),
      paused_(true)
  {
    callback_ = wrapTimerCallback(callback);

    frequency_ = frequency;
    period_ = ros::Duration(1.0 / frequency);

    timer_ops_.period = period_;
    timer_ops_.callback = callback_;
    timer_ops_.callback_queue = &callback_queue_;
    timer_ops_.oneshot = oneshot;
    timer_ops_.autostart = autostart;

    timer_ = std::make_shared<ros::Timer>();
    *timer_ = node_handle_.createTimer(timer_ops_);

    spinner_ = std::make_shared<ros::AsyncSpinner>(1, &callback_queue_);
    spinner_->start();
  }

  ~IsolatedAsyncTimer()
  {
    ROS_DEBUG("IsolatedAsyncTimer destructor");
  }

  void start()
  {
    timer_->start();
  }

  void stop()
  {
    if (stoppable_)
      timer_->stop();
  }

  void pause()
  {
    paused_ = true;
  }

  void resume()
  {
    paused_ = false;
  }

  bool isValid()
  {
    return timer_->isValid();
  }

  void setRate(const float& frequency, bool reset = true)
  {
    frequency_ = frequency;
    period_ = ros::Duration(1.0 / frequency);
    timer_->setPeriod(period_, reset);
  }

  static ros::TimerCallback to_timer_callback(
    const IsolatedAsyncTimer::LambdaCallback& callback)
  {
    return [callback](const ros::TimerEvent & event)
    {
      callback();
    };
  }

private:
  ros::TimerCallback wrapTimerCallback(const ros::TimerCallback& callback) const
  {
    return [ = ](const ros::TimerEvent & ev)
    {
      if (stoppable_ == false || paused_ == false)
      {
        auto last_loop_duration = ev.profile.last_duration.toSec();
        if (ev.last_real.toSec() != 0 && last_loop_duration > period_.toSec())
        {
          auto lag = last_loop_duration - period_.toSec();
          ROS_WARN_STREAM_THROTTLE(1,
            "Missed it's desired rate of " << frequency_ <<
            " [Hz], the loop actually took " << last_loop_duration
            << " [s], which is " << lag << " [s] longer");
        }
        callback(ev);
      }
    };
  }

  ros::NodeHandle node_handle_;

  float frequency_;

  ros::Duration period_;

  ros::TimerOptions timer_ops_;

  ros::TimerCallback callback_;

  ros::CallbackQueue callback_queue_;


  std::shared_ptr<ros::Timer> timer_;

  std::shared_ptr<ros::AsyncSpinner> spinner_;

  std::atomic<bool> stoppable_;

  std::atomic<bool> paused_;
};

}  // namespace robot_activity

#endif  // ROBOT_ACTIVITY_ISOLATED_ASYNC_TIMER_H