transmission_loader.cpp

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.

// ros_control
#include <transmission_interface/transmission_loader.h>


namespace transmission_interface
{

TransmissionLoader::ParseStatus
TransmissionLoader::getActuatorReduction(const TiXmlElement& parent_el,
                                         const std::string&  actuator_name,
                                         const std::string&  transmission_name,
                                         bool                required,
                                         double&             reduction)
{
  // Get XML element
  const TiXmlElement* reduction_el = parent_el.FirstChildElement("mechanicalReduction");
  if(!reduction_el)
  {
    if (required)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << actuator_name << "' of transmission '" << transmission_name <<
                             "' does not specify the required <mechanicalReduction> element.");
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Actuator '" << actuator_name << "' of transmission '" << transmission_name <<
                             "' does not specify the optional <mechanicalReduction> element.");
    }
    return NO_DATA;
  }

  // Cast to number
  try {reduction = std::stod(reduction_el->GetText());}
  catch (const std::logic_error&) // Captures both std::invalid_argument and std::out_of_range
  {
    ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << actuator_name << "' of transmission '" << transmission_name <<
                           "' specifies the <mechanicalReduction> element, but is not a valid number.");
    return BAD_TYPE;
  }
  return SUCCESS;
}

TransmissionLoader::ParseStatus
TransmissionLoader::getJointReduction(const TiXmlElement& parent_el,
                                      const std::string&  joint_name,
                                      const std::string&  transmission_name,
                                      bool                required,
                                      double&             reduction)
{
  // Get XML element
  const TiXmlElement* reduction_el = parent_el.FirstChildElement("mechanicalReduction");
  if(!reduction_el)
  {
    if (required)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' does not specify the required <mechanicalReduction> element.");
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' does not specify the optional <mechanicalReduction> element.");
    }
    return NO_DATA;
  }

  // Cast to number
  try {reduction = std::stod(reduction_el->GetText());}
  catch (const std::logic_error&) // Captures both std::invalid_argument and std::out_of_range
  {
    ROS_ERROR_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                           "' specifies the <mechanicalReduction> element, but is not a valid number.");
    return BAD_TYPE;
  }
  return SUCCESS;
}

TransmissionLoader::ParseStatus
TransmissionLoader::getJointOffset(const TiXmlElement& parent_el,
                                   const std::string&  joint_name,
                                   const std::string&  transmission_name,
                                   bool                required,
                                   double&             offset)
{
  // Get XML element
  const TiXmlElement* offset_el = parent_el.FirstChildElement("offset");
  if(!offset_el)
  {
    if (required)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' does not specify the required <offset> element.");
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' does not specify the optional <offset> element.");
    }
    return NO_DATA;
  }

  // Cast to number
  try {offset = std::stod(offset_el->GetText());}
  catch (const std::logic_error&) // Captures both std::invalid_argument and std::out_of_range
  {
    ROS_ERROR_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                           "' specifies the <offset> element, but is not a valid number.");
    return BAD_TYPE;
  }
  return SUCCESS;
}

TransmissionLoader::ParseStatus
TransmissionLoader::getActuatorRole(const TiXmlElement& parent_el,
                                    const std::string&  actuator_name,
                                    const std::string&  transmission_name,
                                    bool                required,
                                    std::string&        role)
{
  // Get XML element
  const TiXmlElement* role_el = parent_el.FirstChildElement("role");
  if(!role_el)
  {
    if (required)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << actuator_name << "' of transmission '" << transmission_name <<
                             "' does not specify the required <role> element.");
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Actuator '" << actuator_name << "' of transmission '" << transmission_name <<
                             "' does not specify the optional <offset> element.");
    }
    return NO_DATA;
  }

  // Cast to number
  if (!role_el->GetText())
  {
    if (required)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Actuator '" << actuator_name << "' of transmission '" << transmission_name <<
                             "' specifies an empty <role> element.");
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Actuator '" << actuator_name << "' of transmission '" << transmission_name <<
                             "' specifies an empty <role> element.");
    }
    return NO_DATA;
  }
  role = role_el->GetText();

  return SUCCESS;
}

TransmissionLoader::ParseStatus
TransmissionLoader::getJointRole(const TiXmlElement& parent_el,
                                 const std::string&  joint_name,
                                 const std::string&  transmission_name,
                                 bool                required,
                                 std::string&        role)
{
  // Get XML element
  const TiXmlElement* role_el = parent_el.FirstChildElement("role");
  if(!role_el)
  {
    if (required)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' does not specify the required <role> element.");
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' does not specify the optional <offset> element.");
    }
    return NO_DATA;
  }

  // Cast to number
  if (!role_el->GetText())
  {
    if (required)
    {
      ROS_ERROR_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' specifies an empty <role> element.");
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED("parser", "Joint '" << joint_name << "' of transmission '" << transmission_name <<
                             "' specifies an empty <role> element.");
    }
    return NO_DATA;
  }
  role = role_el->GetText();

  return SUCCESS;
}

} // namespace