filter_base.hpp

Go to the documentation of this file.

/*
 * 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 <typeinfo>
#include "ros/assert.h"
#include "ros/console.h"
#include "ros/ros.h"
 
namespace filters
{
 
typedef std::map<std::string, XmlRpc::XmlRpcValue> string_map_t;
 
template<typename T>
class FilterBase
{
public:
  FilterBase():configured_(false){};
 
  virtual ~FilterBase(){};
 
  bool configure(const std::string& param_name, ros::NodeHandle node_handle = ros::NodeHandle())
  {
    XmlRpc::XmlRpcValue config;
    if (!node_handle.getParam(param_name, config))
    {
      ROS_ERROR("Could not find parameter %s on the server, are you sure that it was pushed up correctly?", param_name.c_str());
      return false;
    }
    return configure(config);
    
  }
 
  bool configure(XmlRpc::XmlRpcValue& config)
  {
    if (configured_)
    {
      ROS_WARN("Filter %s of type %s already being reconfigured", filter_name_.c_str(), filter_type_.c_str());
    };
    configured_ = false;
    bool retval = true;
 
    retval = retval && loadConfiguration(config);
    retval = retval && configure();
    configured_ = retval;
    return retval;
  }
 
  virtual bool update(const T& data_in, T& data_out)=0;
 
  std::string getType() {return filter_type_;};
 
  inline const std::string& getName() const {return filter_name_;};
 
 
protected:
 
  virtual bool configure()=0;
 
 
  bool getParam(const std::string& name, std::string& value) const
  {
    string_map_t::const_iterator it = params_.find(name);
    if (it == params_.end())
    {
      return false;
    }
 
    if(it->second.getType() != XmlRpc::XmlRpcValue::TypeString)
    {
      return false;
    }
 
    auto tmp = it->second;
    value = std::string(tmp);
    return true;
  }
 
  bool getParam(const std::string& name, bool& value) const
  {
    string_map_t::const_iterator it = params_.find(name);
    if (it == params_.end())
    {
      return false;
    }
 
    if(it->second.getType() != XmlRpc::XmlRpcValue::TypeBoolean)
    {
      return false;
    }
 
    auto tmp = it->second;
    value = (bool)(tmp);
    return true;
  }
 
  bool getParam(const std::string&name, double& value) const
  {
    string_map_t::const_iterator it = params_.find(name);
    if (it == params_.end())
    {
      return false;
    }
 
    if(it->second.getType() != XmlRpc::XmlRpcValue::TypeDouble && it->second.getType() != XmlRpc::XmlRpcValue::TypeInt)
    {
      return false;
    }
 
    auto tmp = it->second;
    value = it->second.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(tmp) : (double)(tmp);
    return true;
  }
 
  bool getParam(const std::string&name, int& value) const
  {
    string_map_t::const_iterator it = params_.find(name);
    if (it == params_.end())
    {
      return false;
    }
 
    if(it->second.getType() != XmlRpc::XmlRpcValue::TypeInt)
    {
      return false;
    }
 
    auto tmp = it->second;
    value = tmp;
    return true;
  }
 
  bool getParam(const std::string&name, unsigned  int& value) const
  {
    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) const
  {
    string_map_t::const_iterator it = params_.find(name);
    if (it == params_.end())
    {
      return false;
    }
 
    value.clear();
 
    if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      return false;
    }
 
    XmlRpc::XmlRpcValue double_array = it->second;
 
    for (int i = 0; i < double_array.size(); ++i){
      if(double_array[i].getType() != XmlRpc::XmlRpcValue::TypeDouble && double_array[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
      {
        return false;
      }
 
      double double_value = double_array[i].getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(double_array[i]) : (double)(double_array[i]);
      value.push_back(double_value);
    }
    
    return true;
  }
 
  bool getParam(const std::string&name, std::vector<std::string>& value) const
  {
    string_map_t::const_iterator it = params_.find(name);
    if (it == params_.end())
    {
      return false;
    }
 
    value.clear();
 
    if(it->second.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      return false;
    }
 
    XmlRpc::XmlRpcValue string_array = it->second;
    
    for (unsigned int i = 0; i < string_array.size(); ++i){
      if(string_array[i].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
        return false;
      }
 
      value.push_back(string_array[i]);
    }
 
    return true;
  }
 
  bool getParam(const std::string& name, XmlRpc::XmlRpcValue& value) const
  {
    string_map_t::const_iterator it = params_.find(name);
    if (it == params_.end())
    {
      return false;
    }
 
    auto tmp = it->second;
    value = tmp;
    return true;
  }
  
  std::string filter_name_;
  std::string filter_type_;
  bool configured_;
 
  string_map_t params_;
 
private:
  bool setNameAndType(XmlRpc::XmlRpcValue& config)
  {
    if(!config.hasMember("name"))
    {
      ROS_ERROR("Filter didn't have name defined, other strings are not allowed");
      return false;
    }
 
    std::string name = config["name"];
 
    if(!config.hasMember("type"))
    {
      ROS_ERROR("Filter %s didn't have type defined, other strings are not allowed", name.c_str());
      return false;
    }
 
    std::string type = config["type"];
 
    filter_name_ = name;
    filter_type_ = type;
    ROS_DEBUG("Configuring Filter of Type: %s with name %s", type.c_str(), name.c_str());
    return true;
  }
 
protected:
  bool loadConfiguration(XmlRpc::XmlRpcValue& config)
  {
    if(config.getType() != XmlRpc::XmlRpcValue::TypeStruct)
    {
      ROS_ERROR("A filter configuration must be a map with fields name, type, and params");
      return false;
    } 
 
    if (!setNameAndType(config))
    {
      return false;
    }
 
    //check to see if we have parameters in our list
    if(config.hasMember("params"))
    {
      //get the params map
      XmlRpc::XmlRpcValue params = config["params"];
 
      if(params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
      {
        ROS_ERROR("params must be a map");
        return false;
      }
      else{
        //Load params into map
        for(XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it)
        {
          ROS_DEBUG("Loading param %s\n", it->first.c_str());
          params_[it->first] = it->second;
        } 
      }
    }
 
    return true;    
  }
};
 
 
template <typename T>
class MultiChannelFilterBase : public FilterBase<T>
{
public:
  MultiChannelFilterBase():number_of_channels_(0){};
  
  bool configure(unsigned int number_of_channels, const std::string& param_name, ros::NodeHandle node_handle = ros::NodeHandle())
  {
    XmlRpc::XmlRpcValue config;
    if (!node_handle.getParam(param_name, config))
    {
      ROS_ERROR("Could not find parameter %s on the server, are you sure that it was pushed up correctly?", param_name.c_str());
      return false;
    }
    return configure(number_of_channels, config);
    
  }
 
 
  bool configure(unsigned int number_of_channels, XmlRpc::XmlRpcValue& config)
  {
    ROS_DEBUG("FilterBase being configured with XmlRpc xml: %s type: %d", config.toXml().c_str(), config.getType());
    if (configured_)
    {
      ROS_WARN("Filter %s of type %s already being reconfigured", filter_name_.c_str(), filter_type_.c_str());
    };
    configured_ = false;
    number_of_channels_ = number_of_channels;
    ROS_DEBUG("MultiChannelFilterBase configured with %d channels", number_of_channels_);
    bool retval = true;
 
    retval = retval && FilterBase<T>::loadConfiguration(config);
    retval = retval && configure();
    configured_ = retval;
    return retval;
  };
 
 
  bool configure(XmlRpc::XmlRpcValue& config)
  {
    ROS_ERROR("MultiChannelFilterBase configure should be called with a number of channels argument, assuming 1");
    return configure(1, config);
  }
 
  virtual bool configure()=0;
  
 
  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*/)
  {
    ROS_ERROR("THIS IS A MULTI FILTER DON'T CALL SINGLE FORM OF UPDATE");
    return false;
  };
 
 
protected:
  using FilterBase<T>::configured_;
  using FilterBase<T>::filter_type_;
  using FilterBase<T>::filter_name_;
 
  unsigned int number_of_channels_;
  
 
};
 
}
#endif //#ifndef FILTERS_FILTER_BASE_HPP_