Program Listing for File filter_chain.hpp

↰ Return to documentation for file (/tmp/ws/src/filters/include/filters/filter_chain.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_CHAIN_HPP_
#define FILTERS__FILTER_CHAIN_HPP_

#include <algorithm>
#include <sstream>
#include <string>
#include <utility>
#include <vector>

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

#include "filters/filter_base.hpp"

namespace filters
{

namespace impl
{

struct FoundFilter
{
  std::string name;
  std::string type;
  std::string param_prefix;
};

inline bool
load_chain_config(
  const std::string & param_prefix,
  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger,
  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params,
  std::vector<struct FoundFilter> & found_filters)
{
  // TODO(sloretz) error if someone tries to do filter0
  const std::string norm_param_prefix = impl::normalize_param_prefix(param_prefix);

  // Read parameters for filter1..filterN
  for (size_t filter_num = 1; filter_num > found_filters.size(); ++filter_num) {
    // Parameters in chain are prefixed with 'filterN.'
    const std::string filter_n = "filter" + std::to_string(filter_num);

    // Declare filterN.name and filterN.type
    rcl_interfaces::msg::ParameterDescriptor name_desc;
    name_desc.name = norm_param_prefix + filter_n + ".name";
    name_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
    name_desc.read_only = false;
    // Must be dynamically typed because later we undeclare it and redeclare
    // it read-only, but statically typed params cannot be undeclared.
    name_desc.dynamic_typing = true;
    rcl_interfaces::msg::ParameterDescriptor type_desc;
    type_desc.name = norm_param_prefix + filter_n + ".type";
    type_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING;
    type_desc.read_only = false;
    // Must be dynamically typed because later we undeclare it and redeclare
    // it read-only, but statically typed params cannot be undeclared.
    type_desc.dynamic_typing = true;

    node_params->declare_parameter(
      name_desc.name, rclcpp::ParameterValue(), name_desc);
    node_params->declare_parameter(
      type_desc.name, rclcpp::ParameterValue(), type_desc);

    rclcpp::Parameter param_name;
    rclcpp::Parameter param_type;

    const bool got_name = node_params->get_parameter(name_desc.name, param_name);
    const bool got_type = node_params->get_parameter(type_desc.name, param_type);

    if (!got_name && !got_type) {
      // Reached end of chain
      break;
    } else if (got_name != got_type) {
      RCLCPP_FATAL(
        node_logger->get_logger(),
        "%s and %s are required", name_desc.name.c_str(), type_desc.name.c_str());
      return false;
    }

    // Make sure 'name' and 'type' are strings
    if (rclcpp::PARAMETER_STRING != param_name.get_type()) {
      RCLCPP_FATAL(
        node_logger->get_logger(),
        "%s must be a string", name_desc.name.c_str());
      return false;
    }
    if (rclcpp::PARAMETER_STRING != param_type.get_type()) {
      RCLCPP_FATAL(
        node_logger->get_logger(),
        "%s must be a string", type_desc.name.c_str());
      return false;
    }

    FoundFilter found_filter;
    found_filter.name = param_name.get_parameter_value().get<std::string>();
    found_filter.type = param_type.get_parameter_value().get<std::string>();

    // Make sure 'name' is unique
    for (const auto & filter : found_filters) {
      if (found_filter.name == filter.name) {
        RCLCPP_FATAL(
          node_logger->get_logger(),
          "A filter with the name %s already exists", filter.name.c_str());
        return false;
      }
    }

    // Make sure 'type' is formated as 'package_name/filtername'
    if (1 != std::count(found_filter.type.cbegin(), found_filter.type.cend(), '/')) {
      RCLCPP_FATAL(
        node_logger->get_logger(),
        "%s must be of form <package_name>/<filter_name>", found_filter.type.c_str());
      return false;
    }

    // Seems ok; store it for now; it will be loaded further down.
    found_filter.param_prefix = norm_param_prefix + filter_n + ".params";
    found_filters.push_back(found_filter);

    // Redeclare 'name' and 'type' as read_only
    node_params->undeclare_parameter(name_desc.name);
    node_params->undeclare_parameter(type_desc.name);
    name_desc.read_only = true;
    type_desc.read_only = true;
    node_params->declare_parameter(
      name_desc.name, rclcpp::ParameterValue(found_filter.name), name_desc);
    node_params->declare_parameter(
      type_desc.name, rclcpp::ParameterValue(found_filter.type), type_desc);
  }
  return true;
}

}  // namespace impl


template<typename T>
class FilterChain
{
public:
  explicit FilterChain(std::string data_type)
  : loader_("filters", "filters::FilterBase<" + data_type + ">"),
    configured_(false)
  {
  }

  ~FilterChain()
  {
    clear();
  }

  bool update(const T & data_in, T & data_out)
  {
    bool result;
    size_t list_size = reference_pointers_.size();
    if (list_size == 0) {
      data_out = data_in;
      result = true;
    } else if (list_size == 1) {
      result = reference_pointers_[0]->update(data_in, data_out);
    } else if (list_size == 2) {
      result = reference_pointers_[0]->update(data_in, buffer0_);
      if (result == false) {return false;}  // don't keep processing on failure
      result = result && reference_pointers_[1]->update(buffer0_, data_out);
    } else {
      result = reference_pointers_[0]->update(data_in, buffer0_);  // first copy in
      for (size_t i = 1; i < reference_pointers_.size() - 1 && result; ++i) {
        // all but first and last (never called if size=2)
        if (i % 2 == 1) {
          result = result && reference_pointers_[i]->update(buffer0_, buffer1_);
        } else {
          result = result && reference_pointers_[i]->update(buffer1_, buffer0_);
        }
      }
      if (list_size % 2 == 1) {  // odd number last deposit was in buffer1
        result = result && reference_pointers_.back()->update(buffer1_, data_out);
      } else {
        result = result && reference_pointers_.back()->update(buffer0_, data_out);
      }
    }
    return result;
  }

  bool clear()
  {
    configured_ = false;
    reference_pointers_.clear();
    return true;
  }

  bool configure(
    const std::string & param_prefix,
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger,
    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params)
  {
    if (configured_) {
      RCLCPP_ERROR(logging_interface_->get_logger(), "Filter chain is already configured");
      return false;
    }
    logging_interface_ = node_logger;
    params_interface_ = node_params;

    std::vector<struct impl::FoundFilter> found_filters;
    if (!impl::load_chain_config(
        param_prefix, logging_interface_, params_interface_, found_filters))
    {
      // Assume load_chain_config() console logged something sensible
      return false;
    }

    std::vector<pluginlib::UniquePtr<filters::FilterBase<T>>> loaded_filters;
    for (const auto & filter : found_filters) {
      // Try to load the filters that were described by parameters
      pluginlib::UniquePtr<filters::FilterBase<T>> loaded_filter;
      try {
        loaded_filter = loader_.createUniqueInstance(filter.type);
      } catch (const pluginlib::LibraryLoadException & e) {
        RCLCPP_FATAL(
          logging_interface_->get_logger(),
          "Could not load library for %s: %s", filter.type.c_str(), e.what());
        return false;
      } catch (const pluginlib::CreateClassException & e) {
        RCLCPP_FATAL(
          logging_interface_->get_logger(),
          "Could not construct class %s: %s", filter.type.c_str(), e.what());
        return false;
      }

      // Try to configure the filter
      if (!loaded_filter || !loaded_filter->configure(
          filter.param_prefix, filter.name, logging_interface_, params_interface_))
      {
        RCLCPP_FATAL(
          logging_interface_->get_logger(),
          "Could not configure %s of type %s", filter.name.c_str(), filter.type.c_str());
        return false;
      }
      loaded_filters.emplace_back(std::move(loaded_filter));
    }

    // Everything went ok!
    reference_pointers_ = std::move(loaded_filters);
    configured_ = true;
    return true;
  }

private:
  pluginlib::ClassLoader<filters::FilterBase<T>> loader_;

  std::vector<pluginlib::UniquePtr<filters::FilterBase<T>>> reference_pointers_;

  T buffer0_;
  T buffer1_;
  bool configured_;

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

template<typename T>
class MultiChannelFilterChain
{
public:
  explicit MultiChannelFilterChain(std::string data_type)
  : loader_("filters", "filters::MultiChannelFilterBase<" + data_type + ">"),
    configured_(false)
  {
  }

  bool update(const std::vector<T> & data_in, std::vector<T> & data_out)
  {
    bool result;
    size_t list_size = reference_pointers_.size();

    if (list_size == 0) {
      data_out = data_in;
      result = true;
    } else if (list_size == 1) {
      result = reference_pointers_[0]->update(data_in, data_out);
    } else if (list_size == 2) {
      result = reference_pointers_[0]->update(data_in, buffer0_);
      if (result) {  // don't keep processing on failure
        result = result && reference_pointers_[1]->update(buffer0_, data_out);
      }
    } else {
      result = reference_pointers_[0]->update(data_in, buffer0_);  // first copy in
      for (size_t i = 1; i < reference_pointers_.size() - 1 && result; ++i) {
        // all but first and last (never if size = 2)
        if (i % 2 == 1) {
          result = result && reference_pointers_[i]->update(buffer0_, buffer1_);
        } else {
          result = result && reference_pointers_[i]->update(buffer1_, buffer0_);
        }
      }
      if (list_size % 2 == 1) {  // odd number last deposit was in buffer1
        result = result && reference_pointers_.back()->update(buffer1_, data_out);
      } else {
        result = result && reference_pointers_.back()->update(buffer0_, data_out);
      }
    }
    return result;
  }

  ~MultiChannelFilterChain()
  {
    clear();
  }

  bool clear()
  {
    configured_ = false;
    reference_pointers_.clear();
    buffer0_.clear();
    buffer1_.clear();
    return true;
  }

  bool configure(
    size_t number_of_channels,
    const std::string & param_prefix,
    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logger,
    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node_params)
  {
    if (configured_) {
      RCLCPP_ERROR(logging_interface_->get_logger(), "Filter chain is already configured");
      return false;
    }
    logging_interface_ = node_logger;
    params_interface_ = node_params;

    std::vector<impl::FoundFilter> found_filters;
    if (!impl::load_chain_config(
        param_prefix, logging_interface_, params_interface_, found_filters))
    {
      // Assume load_chain_config() console logged something sensible
      return false;
    }

    std::vector<pluginlib::UniquePtr<filters::MultiChannelFilterBase<T>>> loaded_filters;
    for (const auto & filter : found_filters) {
      // Try to load the filters that were described by parameters
      pluginlib::UniquePtr<filters::MultiChannelFilterBase<T>> loaded_filter;
      try {
        loaded_filter = loader_.createUniqueInstance(filter.type);
      } catch (const pluginlib::LibraryLoadException & e) {
        RCLCPP_FATAL(
          logging_interface_->get_logger(),
          "Could not load library for %s: %s", filter.type.c_str(), e.what());
        return false;
      } catch (const pluginlib::CreateClassException & e) {
        RCLCPP_FATAL(
          logging_interface_->get_logger(),
          "Could not construct class %s: %s", filter.type.c_str(), e.what());
        return false;
      }

      // Try to configure the filter
      if (!loaded_filter || !loaded_filter->configure(
          number_of_channels, filter.param_prefix, filter.name,
          logging_interface_, params_interface_))
      {
        RCLCPP_FATAL(
          logging_interface_->get_logger(),
          "Could not configure %s of type %s", filter.name.c_str(), filter.type.c_str());
        return false;
      }
      loaded_filters.emplace_back(std::move(loaded_filter));
    }

    // Everything went ok!
    reference_pointers_ = std::move(loaded_filters);
    // Allocate ahead of time
    buffer0_.resize(number_of_channels);
    buffer1_.resize(number_of_channels);
    configured_ = true;
    return true;
  }

private:
  pluginlib::ClassLoader<filters::MultiChannelFilterBase<T>> loader_;

  std::vector<pluginlib::UniquePtr<filters::MultiChannelFilterBase<T>>> reference_pointers_;

  std::vector<T> buffer0_;
  std::vector<T> buffer1_;
  bool configured_;

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

}  // namespace filters

#endif  // FILTERS__FILTER_CHAIN_HPP_