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.

#pragma once


#include <controller_interface/controller_base.h>
#include <controller_interface/internal/robothw_interfaces.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
{

template <typename... T>
class MultiInterfaceController: public virtual ControllerBase
{
public:
  MultiInterfaceController(bool allow_optional_interfaces = false)
    : allow_optional_interfaces_(allow_optional_interfaces) {}

  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:
  bool initRequest(hardware_interface::RobotHW* robot_hw,
                   ros::NodeHandle&             root_nh,
                   ros::NodeHandle&             controller_nh,
                   ClaimedResources&            claimed_resources) override
  {
    // 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;
  }

  /*\}*/

  static bool hasRequiredInterfaces(hardware_interface::RobotHW* robot_hw)
  {
    return internal::hasInterfaces<T...>(robot_hw);
  }

  static void clearClaims(hardware_interface::RobotHW* robot_hw)
  {
    internal::clearClaims<T...>(robot_hw);
  }

  static void extractInterfaceResources(hardware_interface::RobotHW* robot_hw_in,
                                        hardware_interface::RobotHW* robot_hw_out)
  {
    internal::extractInterfaceResources<T...>(robot_hw_in, robot_hw_out);
  }

  static void populateClaimedResources(hardware_interface::RobotHW* robot_hw,
                                       ClaimedResources&            claimed_resources)
  {
    internal::populateClaimedResources<T...>(robot_hw, claimed_resources);
  }

  hardware_interface::RobotHW robot_hw_ctrl_;

  bool allow_optional_interfaces_;
};

} // namespace