multi_interface_controller.h

Go to the documentation of this file.

// Copyright (C) 2015, PAL Robotics S.L.
//
// 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 names of PAL Robotics S.L. 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.

/* CAUTION: This code was just modified to be build in indigo for personal use.
 * multi_interface_controller is not officially support in indito.*/

#ifndef CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H
#define CONTROLLER_INTERFACE_MULTI_INTERFACE_CONTROLLER_H

#include <algorithm>
#include <sstream>

#include <controller_interface/controller_base.h>
#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/hardware_interface.h>
#include <ros/ros.h>

namespace controller_interface
{

namespace internal
{

template <class T>
bool hasInterface(hardware_interface::RobotHW* robot_hw);

template <class T>
void clearClaims(hardware_interface::RobotHW* robot_hw);

template <class T>
void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in,
                               hardware_interface::RobotHW* robot_hw_out);

template <class T>
void populateClaimedResources(hardware_interface::RobotHW*      robot_hw,
                              std::set<std::string>& claimed_resources);

template <class T>
std::string enumerateElements(const T& val,
                              const std::string& delimiter = ", ",
                              const std::string& prefix = "",
                              const std::string& suffix = "");

} // namespace
template <class T1, class T2 = void, class T3 = void, class T4 = void>
class MultiInterfaceController: public ControllerBase
{
public:
  MultiInterfaceController(bool allow_optional_interfaces = false)
    : allow_optional_interfaces_(allow_optional_interfaces)
  {state_ = CONSTRUCTED;}

  virtual ~MultiInterfaceController() {}

  virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
                    ros::NodeHandle&             /*controller_nh*/)
  {return true;}

  virtual bool init(hardware_interface::RobotHW* /*robot_hw*/,
                    ros::NodeHandle&             /*root_nh*/,
                    ros::NodeHandle&             /*controller_nh*/)
  {return true;}

protected:
  virtual bool initRequest(hardware_interface::RobotHW* robot_hw,
                           ros::NodeHandle&             root_nh,
                           ros::NodeHandle&             controller_nh,
                           std::set<std::string>&       claimed_resources)
  {
    // check if construction finished cleanly
    if (state_ != CONSTRUCTED){
      ROS_ERROR("Cannot initialize this controller because it failed to be constructed");
      return false;
    }

    // check for required hardware interfaces
    if (!allow_optional_interfaces_ && !hasRequiredInterfaces(robot_hw)) {return false;}

    // populate robot hardware abstraction containing only controller hardware interfaces (subset of robot)
    hardware_interface::RobotHW* robot_hw_ctrl_p = &robot_hw_ctrl_;
    extractInterfaceResources(robot_hw, robot_hw_ctrl_p);

    // custom controller initialization
    clearClaims(robot_hw_ctrl_p); // claims will be populated on controller init
    if (!init(robot_hw_ctrl_p, controller_nh) || !init(robot_hw_ctrl_p, root_nh, controller_nh))
    {
      ROS_ERROR("Failed to initialize the controller");
      return false;
    }

    // populate claimed resources
    claimed_resources.clear();
    populateClaimedResources(robot_hw_ctrl_p, claimed_resources);
    clearClaims(robot_hw_ctrl_p);
    // NOTE: Above, claims are cleared since we only want to know what they are and report them back
    // as an output parameter. Actual resource claiming by the controller is done when the controller
    // is start()ed

    // initialization successful
    state_ = INITIALIZED;
    return true;
  }

  /*\}*/

  virtual std::string getHardwareInterfaceType() const
  {
    return hardware_interface::internal::demangledTypeName<T1>();
  }

  static bool hasRequiredInterfaces(hardware_interface::RobotHW* robot_hw)
  {
    using internal::hasInterface;
    return hasInterface<T1>(robot_hw) &&
           hasInterface<T2>(robot_hw) &&
           hasInterface<T3>(robot_hw) &&
           hasInterface<T4>(robot_hw);
  }

  static void clearClaims(hardware_interface::RobotHW* robot_hw)
  {
    using internal::clearClaims;
    clearClaims<T1>(robot_hw);
    clearClaims<T2>(robot_hw);
    clearClaims<T3>(robot_hw);
    clearClaims<T4>(robot_hw);
  }

  static void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in,
                                        hardware_interface::RobotHW* robot_hw_out)
  {
    using internal::extractInterfaceResources;
    extractInterfaceResources<T1>(robot_hw_in, robot_hw_out);
    extractInterfaceResources<T2>(robot_hw_in, robot_hw_out);
    extractInterfaceResources<T3>(robot_hw_in, robot_hw_out);
    extractInterfaceResources<T4>(robot_hw_in, robot_hw_out);
  }

  static void populateClaimedResources(hardware_interface::RobotHW* robot_hw,
                                       std::set<std::string>&       claimed_resources)
  {
    using internal::populateClaimedResources;
    populateClaimedResources<T1>(robot_hw, claimed_resources);
    populateClaimedResources<T2>(robot_hw, claimed_resources);
    populateClaimedResources<T3>(robot_hw, claimed_resources);
    populateClaimedResources<T4>(robot_hw, claimed_resources);
  }

  hardware_interface::RobotHW robot_hw_ctrl_;

  bool allow_optional_interfaces_;

private:
  MultiInterfaceController(const MultiInterfaceController& c);
  MultiInterfaceController& operator =(const MultiInterfaceController& c);
};


namespace internal
{

template <class T>
inline bool hasInterface(hardware_interface::RobotHW* robot_hw)
{
  T* hw = robot_hw->get<T>();
  if (!hw)
  {
    const std::string hw_name = hardware_interface::internal::demangledTypeName<T>();
    ROS_ERROR_STREAM("This controller requires a hardware interface of type '" << hw_name << "', " <<
                     "but is not exposed by the robot.");// Available interfaces in robot:\n" <<
                     //enumerateElements(robot_hw->getNames(), "\n", "- '", "'")); // delimiter, prefix, suffux
    return false;
  }
  return true;
}

// Specialization for unused template parameters defaulting to void
template <>
inline bool hasInterface<void>(hardware_interface::RobotHW* /*robot_hw*/) {return true;}

template <class T>
void clearClaims(hardware_interface::RobotHW* robot_hw)
{
  T* hw = robot_hw->get<T>();
  if (hw) {hw->clearClaims();}
}

// Specialization for unused template parameters defaulting to void
template <>
inline void clearClaims<void>(hardware_interface::RobotHW* /*robot_hw*/) {}

template <class T>
inline void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in,
                                      hardware_interface::RobotHW* robot_hw_out)
{
  T* hw = robot_hw_in->get<T>();
  if (hw) {robot_hw_out->registerInterface(hw);}
}

// Specialization for unused template parameters defaulting to void
template <>
inline void extractInterfaceResources<void>(hardware_interface::RobotHW* /*robot_hw_in*/,
                                            hardware_interface::RobotHW* /*robot_hw_out*/) {}

template <class T>
inline void populateClaimedResources(hardware_interface::RobotHW*      robot_hw,
                                     std::set<std::string>& claimed_resources)
{
  T* hw = robot_hw->get<T>();
  if (hw)
  {
    //hardware_interface::InterfaceResources iface_res;
    //iface_res.hardware_interface = hardware_interface::internal::demangledTypeName<T>();
    //iface_res.resources = hw->getClaims();
    //claimed_resources.push_back(iface_res);

    // TODO: must be fixed so that all claims to be registered
    claimed_resources = hw->getClaims();
  }
}

// Specialization for unused template parameters defaulting to void
template <>
inline void populateClaimedResources<void>(hardware_interface::RobotHW*      /*robot_hw*/,
                                            std::set<std::string>& /*claimed_resources*/) {}

template <class T>
inline std::string enumerateElements(const T&           val,
                                     const std::string& delimiter,
                                     const std::string& prefix,
                                     const std::string& suffix)
{
  std::string ret;
  if (val.empty()) {return ret;}

  const std::string sdp = suffix+delimiter+prefix;
  std::stringstream ss;
  ss << prefix;
  std::copy(val.begin(), val.end(), std::ostream_iterator<typename T::value_type>(ss, sdp.c_str()));
  ret = ss.str();
  if (!ret.empty()) {ret.erase(ret.size() - delimiter.size() - prefix.size());}
  return ret;
}

} // namespace

} // namespace

#endif