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_