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


#include <map>
#include <string>
#include <vector>

#include <hardware_interface/internal/resource_manager.h>
#include <transmission_interface/transmission.h>
#include <transmission_interface/transmission_interface_exception.h>

namespace transmission_interface
{

class TransmissionHandle
{
public:
  std::string getName() const {return name_;}

protected:
  std::string   name_;
  Transmission* transmission_;
  ActuatorData  actuator_data_;
  JointData     joint_data_;

  TransmissionHandle(const std::string&  name,
                     Transmission*       transmission,
                     const ActuatorData& actuator_data,
                     const JointData&    joint_data)
    : name_(name),
      transmission_(transmission),
      actuator_data_(actuator_data),
      joint_data_(joint_data)
  {
    // Precondition: Valid transmission
    if (!transmission_)
    {
      throw TransmissionInterfaceException("Unspecified transmission.");
    }

    // Catch trivial error: All data vectors are empty (handle can't do anything without data)
    if (actuator_data.position.empty() && actuator_data.velocity.empty() && actuator_data.effort.empty() &&
        joint_data.position.empty() && joint_data.velocity.empty() && joint_data.effort.empty())
    {
      throw TransmissionInterfaceException("All data vectors are empty. Transmission instance can't do anything!.");
    }

    // Precondition: All non-empty data vectors must have sizes consistent with the transmission
    if (!actuator_data.position.empty() && actuator_data.position.size() != transmission_->numActuators())
    {
      throw TransmissionInterfaceException("Actuator position data size does not match transmission.");
    }
    if (!actuator_data.velocity.empty() && actuator_data.velocity.size() != transmission_->numActuators())
    {
      throw TransmissionInterfaceException("Actuator velocity data size does not match transmission.");
    }
    if (!actuator_data.effort.empty() && actuator_data.effort.size() != transmission_->numActuators())
    {
      throw TransmissionInterfaceException("Actuator effort data size does not match transmission.");
    }
    if (!actuator_data.absolute_position.empty() && actuator_data.absolute_position.size() != transmission_->numActuators())
    {
      throw TransmissionInterfaceException("Actuator absolute position data size does not match transmission.");
    }
    if (!actuator_data.absolute_position.empty() && actuator_data.absolute_position.size() != transmission_->numActuators())
    {
      throw TransmissionInterfaceException("Actuator torque sensor data size does not match transmission.");
    }

    if (!joint_data.position.empty() && joint_data.position.size() != transmission_->numJoints())
    {
      throw TransmissionInterfaceException("Joint position data size does not match transmission.");
    }
    if (!joint_data.velocity.empty() && joint_data.velocity.size() != transmission_->numJoints())
    {
      throw TransmissionInterfaceException("Joint velocity data size does not match transmission.");
    }
    if (!joint_data.effort.empty() && joint_data.effort.size() != transmission_->numJoints())
    {
      throw TransmissionInterfaceException("Joint effort data size does not match transmission.");
    }
    if (!joint_data.absolute_position.empty() && joint_data.absolute_position.size() != transmission_->numJoints())
    {
      throw TransmissionInterfaceException("Joint absolute position data size does not match transmission.");
    }
    if (!joint_data.torque_sensor.empty() && joint_data.torque_sensor.size() != transmission_->numJoints())
    {
      throw TransmissionInterfaceException("Joint torque sensor data size does not match transmission.");
    }

    // Precondition: Valid pointers to raw data
    if (!hasValidPointers(actuator_data.position))
    {
      throw TransmissionInterfaceException("Actuator position data contains null pointers.");
    }
    if (!hasValidPointers(actuator_data.velocity))
    {
      throw TransmissionInterfaceException("Actuator velocity data contains null pointers.");
    }
    if (!hasValidPointers(actuator_data.effort))
    {
      throw TransmissionInterfaceException("Actuator effort data contains null pointers.");
    }
    if (!actuator_data.absolute_position.empty() && !hasValidPointers(actuator_data.absolute_position))
    {
      throw TransmissionInterfaceException("Actuator absolute position data contains null pointers.");
    }
    if (!actuator_data.torque_sensor.empty() && !hasValidPointers(actuator_data.torque_sensor))
    {
      throw TransmissionInterfaceException("Actuator torque sensor data contains null pointers.");
    }

    if (!hasValidPointers(joint_data.position))
    {
      throw TransmissionInterfaceException("Joint position data contains null pointers.");
    }
    if (!hasValidPointers(joint_data.velocity))
    {
      throw TransmissionInterfaceException("Joint velocity data contains null pointers.");
    }
    if (!hasValidPointers(joint_data.effort))
    {
      throw TransmissionInterfaceException("Joint effort data contains null pointers.");
    }
    if (!joint_data.absolute_position.empty() && !hasValidPointers(joint_data.absolute_position))
    {
      throw TransmissionInterfaceException("Joint absolute position data contains null pointers.");
    }
    if (!joint_data.torque_sensor.empty() && !hasValidPointers(joint_data.torque_sensor))
    {
      throw TransmissionInterfaceException("Joint torque sensor data contains null pointers.");
    }

  }

private:
  static bool hasValidPointers(const std::vector<double*>& data)
  {
    for (const auto& ptr : data)
    {
      if (!ptr) {return false;}
    }
    return true;
  }
};

class ActuatorToJointStateHandle : public TransmissionHandle
{
public:
  ActuatorToJointStateHandle(const std::string&  name,
                             Transmission*       transmission,
                             const ActuatorData& actuator_data,
                             const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate()
  {
    transmission_->actuatorToJointPosition(actuator_data_, joint_data_);
    transmission_->actuatorToJointVelocity(actuator_data_, joint_data_);
    transmission_->actuatorToJointEffort(actuator_data_, joint_data_);

    if(!actuator_data_.absolute_position.empty() && transmission_->hasActuatorToJointAbsolutePosition())
    {
       transmission_->actuatorToJointAbsolutePosition(actuator_data_, joint_data_);
    }

    if(!actuator_data_.torque_sensor.empty() && transmission_->hasActuatorToJointTorqueSensor())
    {
       transmission_->actuatorToJointTorqueSensor(actuator_data_, joint_data_);
    }
  }
  /*\}*/
};


class ActuatorToJointPositionHandle : public TransmissionHandle
{
public:
  ActuatorToJointPositionHandle(const std::string&  name,
                                Transmission*       transmission,
                                const ActuatorData& actuator_data,
                                const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate() {transmission_->actuatorToJointPosition(actuator_data_, joint_data_);}
  /*\}*/
};


class ActuatorToJointVelocityHandle : public TransmissionHandle
{
public:
  ActuatorToJointVelocityHandle(const std::string&  name,
                                Transmission*       transmission,
                                const ActuatorData& actuator_data,
                                const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate() {transmission_->actuatorToJointVelocity(actuator_data_, joint_data_);}
  /*\}*/
};


class ActuatorToJointEffortHandle : public TransmissionHandle
{
public:
  ActuatorToJointEffortHandle(const std::string&  name,
                              Transmission*       transmission,
                              const ActuatorData& actuator_data,
                              const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate() {transmission_->actuatorToJointEffort(actuator_data_, joint_data_);}
  /*\}*/
};


class JointToActuatorStateHandle : public TransmissionHandle
{
public:
  JointToActuatorStateHandle(const std::string&  name,
                             Transmission*       transmission,
                             const ActuatorData& actuator_data,
                             const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate()
  {
    transmission_->jointToActuatorPosition(joint_data_, actuator_data_);
    transmission_->jointToActuatorVelocity(joint_data_, actuator_data_);
    transmission_->jointToActuatorEffort(  joint_data_, actuator_data_);
  }
  /*\}*/
};


class JointToActuatorPositionHandle : public TransmissionHandle
{
public:
  JointToActuatorPositionHandle(const std::string&  name,
                                Transmission*       transmission,
                                const ActuatorData& actuator_data,
                                const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate() {transmission_->jointToActuatorPosition(joint_data_, actuator_data_);}
  /*\}*/
};


class JointToActuatorVelocityHandle : public TransmissionHandle
{
public:
  JointToActuatorVelocityHandle(const std::string&  name,
                                Transmission*       transmission,
                                const ActuatorData& actuator_data,
                                const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate() {transmission_->jointToActuatorVelocity(joint_data_, actuator_data_);}
  /*\}*/
};


class JointToActuatorEffortHandle : public TransmissionHandle
{
public:
  JointToActuatorEffortHandle(const std::string&  name,
                              Transmission*       transmission,
                              const ActuatorData& actuator_data,
                              const JointData&    joint_data)
    : TransmissionHandle(name, transmission, actuator_data, joint_data) {}

  void propagate() {transmission_->jointToActuatorEffort(joint_data_, actuator_data_);}
  /*\}*/
};

template <class HandleType>
class TransmissionInterface : public hardware_interface::ResourceManager<HandleType>
{
public:

  HandleType getHandle(const std::string& name)
  {
    // Rethrow exception with a meaningful type
    try
    {
      return this->hardware_interface::ResourceManager<HandleType>::getHandle(name);
    }
    catch(const std::logic_error& e)
    {
      throw TransmissionInterfaceException(e.what());
    }
  }

  void propagate()
  {
    for (auto&& resource_name_and_handle : this->resource_map_)
    {
      resource_name_and_handle.second.propagate();
    }
  }
  /*\}*/
};

// Convenience typedefs

class ActuatorToJointStateInterface : public TransmissionInterface<ActuatorToJointStateHandle> {};

class ActuatorToJointPositionInterface : public TransmissionInterface<ActuatorToJointPositionHandle> {};

class ActuatorToJointVelocityInterface : public TransmissionInterface<ActuatorToJointVelocityHandle> {};

class ActuatorToJointEffortInterface : public TransmissionInterface<ActuatorToJointEffortHandle> {};

class JointToActuatorStateInterface : public TransmissionInterface<JointToActuatorStateHandle> {};

class JointToActuatorPositionInterface : public TransmissionInterface<JointToActuatorPositionHandle> {};

class JointToActuatorVelocityInterface : public TransmissionInterface<JointToActuatorVelocityHandle> {};

class JointToActuatorEffortInterface : public TransmissionInterface<JointToActuatorEffortHandle> {};

} // transmission_interface