Program Listing for File timer_impl.h

↰ Return to documentation for file (include/swri_roscpp/timer_impl.h)

// *****************************************************************************
//
// Copyright (c) 2015, Southwest Research Institute® (SwRI®)
// 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 Southwest Research Institute® (SwRI®) 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 SOUTHWEST RESEARCH INSTITUTE 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 SWRI_ROSCPP_TIMER_IMPL_H_
#define SWRI_ROSCPP_TIMER_IMPL_H_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/time.hpp>
#include <chrono>

namespace swri
{
class Timer;
class TimerImpl
{
 protected:
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Duration desired_period_ = rclcpp::Duration(0, 0);

  int ticks_;

  typedef std::chrono::nanoseconds WallDuration;
  typedef std::chrono::system_clock::time_point WallTime;

  WallTime tick_begin_wall_;
  rclcpp::Time tick_begin_normal_;

  rclcpp::Duration total_periods_ = rclcpp::Duration(std::chrono::nanoseconds::zero());
  rclcpp::Duration min_period_ = rclcpp::Duration(std::chrono::nanoseconds::zero());
  rclcpp::Duration max_period_ = rclcpp::Duration(std::chrono::nanoseconds::zero());

  WallDuration total_durations_;
  WallDuration min_duration_;
  WallDuration max_duration_;

  void tickBegin()
  {
    tick_begin_wall_ = std::chrono::system_clock::now();

    rclcpp::Time now = rclcpp::Clock().now();
    if (ticks_ > 0) {
      rclcpp::Duration period = now - tick_begin_normal_;
      total_periods_ = total_periods_ + period;

      if (ticks_ == 1) {
        min_period_ = period;
        max_period_ = period;
      } else {
        min_period_ = std::min(min_period_, period);
        max_period_ = std::max(max_period_, period);
      }
    }
    tick_begin_normal_ = now;
  }

  void tickEnd()
  {
    WallTime end_time_ = std::chrono::system_clock::now();
    WallDuration duration = end_time_ - tick_begin_wall_;
    total_durations_ += duration;
    if (ticks_ == 0) {
      min_duration_ = duration;
      max_duration_ = duration;
    } else {
      min_duration_ = std::min(min_duration_, duration);
      max_duration_ = std::max(max_duration_, duration);
    }
    ticks_++;
  }

 public:
  TimerImpl()
  {
    resetStatistics();
  }

  rclcpp::Duration desiredPeriod() const
  {
    return desired_period_;
  }

  void resetStatistics()
  {
    ticks_ = 0;
  }

  // The number of times the timer callback has been called.
  size_t ticks() const
  {
    return ticks_;
  }

  double meanFrequencyHz() const
  {
    if (ticks_ < 2) {
      return 0.0;
    } else {
      return 1e9 / meanPeriod().nanoseconds();
    }
  }

  rclcpp::Duration meanPeriod() const
  {
    if (ticks_ < 2) {
      return rclcpp::Duration::max();
    } else {
      return rclcpp::Duration::from_seconds(total_periods_.seconds() / (ticks_ - 1));
    }
  }

  rclcpp::Duration minPeriod() const
  {
    if (ticks_ < 2) {
      return rclcpp::Duration::max();
    } else {
      return min_period_;
    }
  }

  rclcpp::Duration maxPeriod() const
  {
    if (ticks_ < 2) {
      return rclcpp::Duration::max();
    } else {
      return max_period_;
    }
  }

  WallDuration meanDuration() const
  {
    if (ticks_ == 0) {
      return WallDuration(0);
    } else {
      return WallDuration(total_durations_.count() / 100000000 / ticks_);
    }
  }

  WallDuration minDuration() const
  {
    if (ticks_ == 0) {
      return WallDuration(0);
    } else {
      return min_duration_;
    }
  }

  WallDuration maxDuration() const
  {
    if (ticks_ == 0) {
      return WallDuration(0);
    } else {
      return max_duration_;
    }
  }
};  // class TimerImpl

template<class T>
class TypedTimerImpl : public TimerImpl
{
  T *obj_;
  void (T::*callback_)();

 public:
  TypedTimerImpl(
    rclcpp::Node &nh,
    rclcpp::Duration period,
    void(T::*callback)(),
    T *obj)
  {
    callback_ = callback;
    obj_ = obj;

    desired_period_ = period;
    timer_ = nh.create_wall_timer(std::chrono::nanoseconds(period.nanoseconds()),
                            std::bind(&TypedTimerImpl::handleTimer,
                            this));
  }

  void handleTimer()
  {
    tickBegin();
    (obj_->*callback_)();
    tickEnd();
  }
};  // class TypedTimerImpl
}  // namespace swri
#endif  // SWRI_ROSCPP_TIMER_IMPL_H_