signal_filter.hpp

Go to the documentation of this file.

// Copyright 2020 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
#ifndef SIGNAL_FILTERS__SIGNAL_FILTER_HPP_
#define SIGNAL_FILTERS__SIGNAL_FILTER_HPP_

#include <common/types.hpp>
#include <signal_filters/visibility_control.hpp>

#include <algorithm>
#include <chrono>
#include <cmath>
#include <stdexcept>
#include <type_traits>

using autoware::common::types::bool8_t;

namespace autoware
{
namespace common
{
namespace signal_filters
{

struct DummyClock
{
  static auto now() {return std::chrono::steady_clock::now();}  // Just for test convenience
  using time_point = std::chrono::steady_clock::time_point;  // Just for test convenience
};

template<typename T, typename ClockT = DummyClock>
class SIGNAL_FILTERS_PUBLIC FilterBase
{
  static_assert(std::is_floating_point<T>::value, "Filters require a floating point type");
  constexpr static bool8_t use_time_point_api = !std::is_same<ClockT, DummyClock>::value;

public:
  using clock_type = ClockT;
  using signal_type = T;

  virtual ~FilterBase() = default;
  template<typename DummyT = T, typename = std::enable_if_t<use_time_point_api, DummyT>>
  T filter(T value, typename clock_type::time_point time_stamp)
  {
    const auto dt = time_stamp - m_last_observation_stamp;
    const auto ret = filter_impl_checked(value, dt);
    m_last_observation_stamp = time_stamp;  // Goes after just in case there's an exception
    return ret;
  }
  template<typename DummyT = T, typename = std::enable_if_t<!use_time_point_api, DummyT>>
  T filter(T value, std::chrono::nanoseconds duration)
  {
    return filter_impl_checked(value, duration);
  }

protected:
  T filter_impl_checked(T value, std::chrono::nanoseconds duration)
  {
    if (decltype(duration)::zero() >= duration) {
      throw std::domain_error{"Duration is negative"};
    }
    if (!std::isfinite(value)) {
      throw std::domain_error{"Value is not finite"};
    }
    return filter_impl(value, duration);
  }
  virtual T filter_impl(T value, std::chrono::nanoseconds duration) = 0;

private:
  typename clock_type::time_point m_last_observation_stamp{};
};

}  // namespace signal_filters
}  // namespace common
}  // namespace autoware

#endif  // SIGNAL_FILTERS__SIGNAL_FILTER_HPP_