interface_manager.h

Go to the documentation of this file.

// Copyright (C) 2013, PAL Robotics S.L.
// Copyright (C) 2012, hiDOF INC.
//
// 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 hiDOF, 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 HARDWARE_INTERFACE_INTERFACE_MANAGER_H
#define HARDWARE_INTERFACE_INTERFACE_MANAGER_H

#include <map>
#include <string>
#include <vector>
#include <boost/ptr_container/ptr_vector.hpp>

#include <ros/console.h>

#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/internal/resource_manager.h>

namespace hardware_interface
{

namespace internal
{

// SFINAE workaround, so that we have reflection inside the template functions
template <typename T>
struct CheckIsResourceManager {
  // method called if C is a ResourceManager
  template <typename C>
  static void callCM(typename std::vector<C*>& managers, C* result, typename C::resource_manager_type*)
  {
    std::vector<typename C::resource_manager_type*> managers_in;
    // we have to typecase back to base class
    for(typename std::vector<C*>::iterator it = managers.begin(); it != managers.end(); ++it)
      managers_in.push_back(static_cast<typename C::resource_manager_type*>(*it));
    C::concatManagers(managers_in, result);
  }

  // method called if C is not a ResourceManager
  template <typename C>
  static void callCM(typename std::vector<C*>& managers, C* result, ...) {}

  // calls ResourceManager::concatManagers if C is a ResourceManager
  static const void callConcatManagers(typename std::vector<T*>& managers, T* result)
  { callCM<T>(managers, result, 0); }


  // method called if C is a ResourceManager
  template <typename C>
  static void callGR(std::vector<std::string> &resources, C* iface, typename C::resource_manager_type*)
  {
    resources = iface->getNames();
  }

  // method called if C is not a ResourceManager
  template <typename C>
  static void callGR(std::vector<std::string> &resources, T* iface, ...) { }

  // calls ResourceManager::concatManagers if C is a ResourceManager
  static void callGetResources(std::vector<std::string> &resources, T* iface)
  { return callGR<T>(resources, iface, 0); }

  template <typename C>
  static T* newCI(boost::ptr_vector<ResourceManagerBase> &guards, typename C::resource_manager_type*)
  {
    T* iface_combo = new T;
    // save the new interface pointer to allow for its correct destruction
    guards.push_back(static_cast<ResourceManagerBase*>(iface_combo));
    return iface_combo;
  }

  // method called if C is not a ResourceManager
  template <typename C>
  static T* newCI(boost::ptr_vector<ResourceManagerBase> &guards, ...) {
    // it is not a ResourceManager
    ROS_ERROR("You cannot register multiple interfaces of the same type which are "
              "not of type ResourceManager. There is no established protocol "
              "for combining them.");
    return NULL;
  }

  static T* newCombinedInterface(boost::ptr_vector<ResourceManagerBase> &guards)
  {
    return newCI<T>(guards, 0);
  }

};

} // namespace internal

class InterfaceManager
{
public:
  template<class T>
  void registerInterface(T* iface)
  {
    const std::string iface_name = internal::demangledTypeName<T>();
    if (interfaces_.find(iface_name) != interfaces_.end())
    {
      ROS_WARN_STREAM("Replacing previously registered interface '" << iface_name << "'.");
    }
    interfaces_[iface_name] = iface;
    internal::CheckIsResourceManager<T>::callGetResources(resources_[iface_name], iface);
  }

  void registerInterfaceManager(InterfaceManager* iface_man)
  {
    interface_managers_.push_back(iface_man);
  }

  template<class T>
  T* get()
  {
    std::string type_name = internal::demangledTypeName<T>();
    std::vector<T*> iface_list;

    // look for interfaces registered here
    InterfaceMap::iterator it = interfaces_.find(type_name);
    if (it != interfaces_.end()) {
      T* iface = static_cast<T*>(it->second);
      if (!iface) {
        ROS_ERROR_STREAM("Failed reconstructing type T = '" << type_name.c_str() <<
                         "'. This should never happen");
        return NULL;
      }
      iface_list.push_back(iface);
    }

    // look for interfaces registered in the registered hardware
    for(InterfaceManagerVector::iterator it = interface_managers_.begin();
        it != interface_managers_.end(); ++it) {
      T* iface = (*it)->get<T>();
      if (iface)
        iface_list.push_back(iface);
    }

    if(iface_list.size() == 0)
      return NULL;

    if(iface_list.size() == 1)
      return iface_list.front();

    // if we're here, we have multiple interfaces, and thus we must construct a new
    // combined interface, or return one already constructed
    T* iface_combo;
    InterfaceMap::iterator it_combo = interfaces_combo_.find(type_name);
    if(it_combo != interfaces_combo_.end() &&
        num_ifaces_registered_[type_name] == iface_list.size()) {
      // there exists a combined interface with the same number of interfaces combined
      // (since you cannot unregister interfaces, this will be guaranteed to be the
      //  same interfaces from previous calls)
      iface_combo = static_cast<T*>(it_combo->second);
    } else {
      // no existing combined interface
      iface_combo = internal::CheckIsResourceManager<T>::newCombinedInterface(interface_destruction_list_);
      if(iface_combo) {
        // concat all of the resource managers together
        internal::CheckIsResourceManager<T>::callConcatManagers(iface_list, iface_combo);
        // save the combined interface for if this is called again
        interfaces_combo_[type_name] = iface_combo;
        num_ifaces_registered_[type_name] = iface_list.size();
      } else {
        // it is not a ResourceManager
        ROS_ERROR("You cannot register multiple interfaces of the same type which are "
                  "not of type ResourceManager. There is no established protocol "
                  "for combining them.");
        iface_combo = NULL;
      }
    }
    return iface_combo;
  }

  std::vector<std::string> getNames() const
  {
    std::vector<std::string> out;
    out.reserve(interfaces_.size());
    for(InterfaceMap::const_iterator it = interfaces_.begin(); it != interfaces_.end(); ++it)
    {
      out.push_back(it->first);
    }
    return out;
  }

  std::vector<std::string> getInterfaceResources(std::string iface_type) const
  {
    std::vector<std::string> out;
    ResourceMap::const_iterator it = resources_.find(iface_type);
    if(it != resources_.end())
    {
      out = it->second;
    }
    return out;
  }

protected:
  typedef std::map<std::string, void*> InterfaceMap;
  typedef std::vector<InterfaceManager*> InterfaceManagerVector;
  typedef std::map<std::string, size_t> SizeMap;
  typedef std::map<std::string, std::vector<std::string> > ResourceMap;

  InterfaceMap interfaces_;
  InterfaceMap interfaces_combo_;
  InterfaceManagerVector interface_managers_;
  SizeMap num_ifaces_registered_;
  boost::ptr_vector<ResourceManagerBase> interface_destruction_list_;
  ResourceMap resources_;
};

} // namespace

#endif // header guard