Program Listing for File filter_base.hpp

↰ Return to documentation for file (/tmp/ws/src/filters/include/filters/filter_base.hpp)

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * 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 the 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 FILTERS__FILTER_BASE_HPP_
#define FILTERS__FILTER_BASE_HPP_

#include <string>
#include <typeinfo>
#include <vector>

#include "rcl_interfaces/msg/parameter_descriptor.hpp"
#include "rcl_interfaces/msg/parameter_type.hpp"
#include "rclcpp/rclcpp.hpp"

namespace filters
{

namespace impl
{

inline std::string normalize_param_prefix(std::string prefix)
{
  if (!prefix.empty()) {
    if ('.' != prefix.back()) {
      prefix += '.';
    }
  }
  return prefix;
}

}  // namespace impl

template<typename T>
class FilterBase
{
public:
  FilterBase()
  : configured_(false) {}

  virtual ~FilterBase() = default;

  bool configure(
    const std::string & param_prefix,
    const std::string & filter_name,
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger,
    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params)
  {
    if (configured_) {
      RCLCPP_WARN(
        node_logger->get_logger(),
        "Filter %s already being reconfigured",
        filter_name_.c_str());
    }
    if (!node_params) {
      throw std::runtime_error("Need a parameters interface to function.");
    }

    configured_ = false;

    filter_name_ = filter_name;
    param_prefix_ = impl::normalize_param_prefix(param_prefix);
    params_interface_ = node_params;
    logging_interface_ = node_logger;

    configured_ = configure();
    return configured_;
  }

  virtual bool update(const T & data_in, T & data_out) = 0;

  inline const std::string & getName() {return filter_name_;}

private:
  template<typename PT>
  bool getParamImpl(const std::string & name, const uint8_t type, PT default_value, PT & value_out)
  {
    std::string param_name = param_prefix_ + name;

    if (!params_interface_->has_parameter(param_name)) {
      // Declare parameter
      rclcpp::ParameterValue default_parameter_value(default_value);
      rcl_interfaces::msg::ParameterDescriptor desc;
      desc.name = name;
      desc.type = type;
      desc.read_only = true;

      if (name.empty()) {
        throw std::runtime_error("Parameter must have a name");
      }

      params_interface_->declare_parameter(param_name, default_parameter_value, desc);
    }

    value_out = params_interface_->get_parameter(param_name).get_parameter_value().get<PT>();
    // TODO(sloretz) seems to be no way to tell if parameter was initialized or not
    return true;
  }

protected:
  virtual bool configure() = 0;

  bool getParam(const std::string & name, std::string & value)
  {
    return getParamImpl(
      name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING, std::string(), value);
  }

  bool getParam(const std::string & name, bool & value)
  {
    return getParamImpl(name, rcl_interfaces::msg::ParameterType::PARAMETER_BOOL, false, value);
  }

  bool getParam(const std::string & name, double & value)
  {
    return getParamImpl(name, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, 0.0, value);
  }

  bool getParam(const std::string & name, int & value)
  {
    return getParamImpl(name, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER, 0, value);
  }

  bool getParam(const std::string & name, unsigned int & value)
  {
    int signed_value;
    if (!getParam(name, signed_value)) {
      return false;
    }
    if (signed_value < 0) {
      return false;
    }
    value = signed_value;
    return true;
  }

  bool getParam(const std::string & name, size_t & value)
  {
    int signed_value;
    if (!getParam(name, signed_value)) {
      return false;
    }
    if (signed_value < 0) {
      return false;
    }
    value = signed_value;
    return true;
  }

  bool getParam(const std::string & name, std::vector<double> & value)
  {
    return getParamImpl(
      name, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, {}, value);
  }

  bool getParam(const std::string & name, std::vector<std::string> & value)
  {
    return getParamImpl(
      name, rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, {}, value);
  }

  std::string filter_name_;
  bool configured_;

  std::string param_prefix_;

  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr params_interface_;
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logging_interface_;
};


template<typename T>
class MultiChannelFilterBase : public FilterBase<T>
{
public:
  MultiChannelFilterBase()
  : number_of_channels_(0)
  {
  }

  virtual ~MultiChannelFilterBase() = default;

  bool configure(
    size_t number_of_channels,
    const std::string & param_prefix,
    const std::string & filter_name,
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger,
    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params)
  {
    number_of_channels_ = number_of_channels;

    return FilterBase<T>::configure(param_prefix, filter_name, node_logger, node_params);
  }

  virtual bool update(const std::vector<T> & data_in, std::vector<T> & data_out) = 0;

  virtual bool update(const T & /*data_in*/, T & /*data_out*/)
  {
    RCLCPP_ERROR(
      this->logging_interface_->get_logger(),
      "THIS IS A MULTI FILTER DON'T CALL SINGLE FORM OF UPDATE");
    return false;
  }

protected:
  size_t number_of_channels_;
};

}  // namespace filters

#endif  // FILTERS__FILTER_BASE_HPP_