robot_hw.h

Go to the documentation of this file.

// 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.

#pragma once


#include <list>
#include <map>
#include <memory>
#include <typeinfo>
#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/internal/interface_manager.h>
#include <hardware_interface/hardware_interface.h>
#include <hardware_interface/controller_info.h>
#include <ros/console.h>
#include <ros/node_handle.h>

namespace hardware_interface
{

class RobotHW : public InterfaceManager
{
public:
  virtual ~RobotHW() = default;

  virtual bool init(ros::NodeHandle& /*root_nh*/, ros::NodeHandle &/*robot_hw_nh*/) {return true;}

  virtual bool checkForConflict(const std::list<ControllerInfo>& info) const
  {
    // Map from resource name to all controllers claiming it
    std::map<std::string, std::list<ControllerInfo>> resource_map;

    // Populate a map of all controllers claiming individual resources.
    // We do this by iterating over every claimed resource of every hardware interface used by every controller
    for (const auto& controller_info : info)
    {
      for (const auto& claimed_resource : controller_info.claimed_resources)
      {
        for (const auto& iface_resource : claimed_resource.resources)
        {
          resource_map[iface_resource].push_back(controller_info);
        }
      }
    }

    // Enforce resource exclusivity policy: No resource can be claimed by more than one controller
    bool in_conflict = false;
    for (const auto& resource_name_and_claiming_controllers : resource_map)
    {
      if (resource_name_and_claiming_controllers.second.size() > 1)
      {
        std::string controller_list;
        for (const auto& controller : resource_name_and_claiming_controllers.second)
          controller_list += controller.name + ", ";
        ROS_WARN("Resource conflict on [%s].  Controllers = [%s]", resource_name_and_claiming_controllers.first.c_str(), controller_list.c_str());
        in_conflict = true;
      }
    }

    return in_conflict;
  }
  virtual bool prepareSwitch(const std::list<ControllerInfo>& /*start_list*/,
                             const std::list<ControllerInfo>& /*stop_list*/) { return true; }

  virtual void doSwitch(const std::list<ControllerInfo>& /*start_list*/,
                        const std::list<ControllerInfo>& /*stop_list*/) {}

  enum SwitchState
  {
    DONE,
    ONGOING,
    ERROR
  };

  virtual SwitchState switchResult() const
  {
    return DONE;
  }

  virtual SwitchState switchResult(const ControllerInfo& /*controller*/) const
  {
    return DONE;
  }
  virtual void read(const ros::Time& /*time*/, const ros::Duration& /*period*/) {}

  virtual void write(const ros::Time& /*time*/, const ros::Duration& /*period*/) {}

};

typedef std::shared_ptr<RobotHW> RobotHWSharedPtr;

}