transmission_interface_loader.h

Go to the documentation of this file.

// Copyright (C) 2013, 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 name 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


// C++ standard
#include <algorithm>
#include <limits>
#include <map>
#include <string>
#include <utility>
#include <vector>
#include <memory>

// ROS
#include <ros/console.h>

// pluginlib
#include <pluginlib/class_loader.hpp>

// ros_control
#include <hardware_interface/actuator_state_interface.h>
#include <hardware_interface/actuator_command_interface.h>
#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>

#include <transmission_interface/robot_transmissions.h>
#include <transmission_interface/transmission.h>
#include <transmission_interface/transmission_interface.h>
#include <transmission_interface/transmission_info.h>
#include <transmission_interface/transmission_loader.h>
#include <transmission_interface/transmission_parser.h>

namespace transmission_interface
{

// TODO: Don't assume interfaces?
struct RawJointData
{
  double position          = {std::numeric_limits<double>::quiet_NaN()};
  double velocity          = {std::numeric_limits<double>::quiet_NaN()};
  double effort            = {std::numeric_limits<double>::quiet_NaN()};
  double position_cmd      = {std::numeric_limits<double>::quiet_NaN()};
  double velocity_cmd      = {std::numeric_limits<double>::quiet_NaN()};
  double effort_cmd        = {std::numeric_limits<double>::quiet_NaN()};
  double absolute_position = {std::numeric_limits<double>::quiet_NaN()};
  double torque_sensor     = {std::numeric_limits<double>::quiet_NaN()};

  bool hasAbsolutePosition = {true};
  bool hasTorqueSensor     = {true};
};

typedef std::map<std::string, RawJointData> RawJointDataMap;

struct JointInterfaces
{
  hardware_interface::JointStateInterface    joint_state_interface;
  hardware_interface::PositionJointInterface position_joint_interface;
  hardware_interface::VelocityJointInterface velocity_joint_interface;
  hardware_interface::EffortJointInterface   effort_joint_interface;
};

struct ForwardTransmissionInterfaces
{
  ActuatorToJointStateInterface    act_to_jnt_state;
  JointToActuatorPositionInterface jnt_to_act_pos_cmd;
  JointToActuatorVelocityInterface jnt_to_act_vel_cmd;
  JointToActuatorEffortInterface   jnt_to_act_eff_cmd;
};

struct InverseTransmissionInterfaces
{
  JointToActuatorStateInterface    jnt_to_act_state;
  ActuatorToJointPositionInterface act_to_jnt_pos_cmd;
  ActuatorToJointVelocityInterface act_to_jnt_vel_cmd;
  ActuatorToJointEffortInterface   act_to_jnt_eff_cmd;
};

struct TransmissionLoaderData
{
  hardware_interface::RobotHW*  robot_hw            = {nullptr}; 
  RobotTransmissions*           robot_transmissions = {nullptr}; 
  JointInterfaces               joint_interfaces;
  RawJointDataMap               raw_joint_data_map;
  ForwardTransmissionInterfaces transmission_interfaces;
  InverseTransmissionInterfaces inverse_transmission_interfaces;
  std::vector<TransmissionSharedPtr>  transmission_data;
};

class RequisiteProvider // TODO: There must be a more descriptive name for this class!
{
public:
  virtual ~RequisiteProvider() = default;

  virtual bool updateJointInterfaces(const TransmissionInfo&      transmission_info,
                                     hardware_interface::RobotHW* robot_hw,
                                     JointInterfaces&             joint_interfaces,
                                     RawJointDataMap&             raw_joint_data_map) = 0;

  bool loadTransmissionMaps(const TransmissionInfo& transmission_info,
                            TransmissionLoaderData& loader_data,
                            TransmissionSharedPtr   transmission);
protected:
  struct TransmissionHandleData
  {
    std::string           name;
    ActuatorData          act_state_data;
    ActuatorData          act_cmd_data;
    JointData             jnt_state_data;
    JointData             jnt_cmd_data;
    TransmissionSharedPtr transmission;
  };

  virtual bool getJointStateData(const TransmissionInfo& transmission_info,
                                 const RawJointDataMap&  raw_joint_data_map,
                                 JointData&              jnt_state_data) = 0;

  virtual bool getJointCommandData(const TransmissionInfo& transmission_info,
                                   const RawJointDataMap&  raw_joint_data_map,
                                   JointData&              jnt_cmd_data) = 0;

  virtual bool getActuatorStateData(const TransmissionInfo&      transmission_info,
                                    hardware_interface::RobotHW* robot_hw,
                                    ActuatorData&                act_state_data) = 0;

  virtual bool getActuatorCommandData(const TransmissionInfo&      transmission_info,
                                      hardware_interface::RobotHW* robot_hw,
                                      ActuatorData&                act_cmd_data) = 0;

  virtual bool registerTransmission(TransmissionLoaderData& loader_data,
                                    TransmissionHandleData& handle_data) = 0;

  template <class Interface>
  static bool hasResource(const std::string& name, const Interface& iface)
  {
    using hardware_interface::internal::demangledTypeName;

    // Do nothing if resource already exists on the interface
    const std::vector<std::string>& existing_resources = iface.getNames();
    if (existing_resources.end() != std::find(existing_resources.begin(), existing_resources.end(), name))
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Resource '" << name << "' already exists on interface '" <<
                             demangledTypeName<Interface>());
      return true;
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Resource '" << name << "' does not exist on interface '" <<
                             demangledTypeName<Interface>());
      return false;
    }
  }

  template <class HardwareInterface, class Handle>
  bool getActuatorHandles(const std::vector<ActuatorInfo>& actuators_info,
                          hardware_interface::RobotHW*     robot_hw,
                          std::vector<Handle>&             handles)
  {
    using hardware_interface::RobotHW;
    using hardware_interface::HardwareInterfaceException;
    using hardware_interface::internal::demangledTypeName;

    HardwareInterface* hw_iface = robot_hw->get<HardwareInterface>();

    // Check required hardware interface
    if (!hw_iface)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Robot does not have the required hardware interface '" <<
                             demangledTypeName<HardwareInterface>() << "'.");
      return false;
    }

    // Get handles to all required resource
    for (const auto& info : actuators_info)
    {
      try
      {
        handles.push_back(hw_iface->getHandle(info.name_));
      }
      catch(const HardwareInterfaceException& ex)
      {
        ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << info.name_ <<
                               "' does not expose the required hardware interface '" <<
                               demangledTypeName<HardwareInterface>() << "'.");
        return false;
      }
    }
    return true;
  }
};

class TransmissionInterfaceLoader
{
public:
  TransmissionInterfaceLoader(hardware_interface::RobotHW* robot_hw,
                              RobotTransmissions*          robot_transmissions);

  bool load(const std::string& urdf);

  bool load(const std::vector<TransmissionInfo>& transmission_info_vec);

  bool load(const TransmissionInfo& transmission_info);

  TransmissionLoaderData* getData() {return &loader_data_;}

private:
  typedef pluginlib::ClassLoader<TransmissionLoader>      TransmissionClassLoader;
  typedef std::shared_ptr<TransmissionClassLoader>      TransmissionClassLoaderPtr;
  typedef pluginlib::ClassLoader<RequisiteProvider>       RequisiteProviderClassLoader;
  typedef std::shared_ptr<RequisiteProviderClassLoader> RequisiteProviderClassLoaderPtr;

  typedef std::shared_ptr<RequisiteProvider>            RequisiteProviderPtr;

  TransmissionClassLoaderPtr transmission_class_loader_;
  RequisiteProviderClassLoaderPtr req_provider_loader_;

private:
  hardware_interface::RobotHW* robot_hw_ptr_;
  RobotTransmissions*          robot_transmissions_ptr_;

  TransmissionLoaderData loader_data_;
};

} // namespace