DynamicParameterClass.cpp

Go to the documentation of this file.

// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// This file is part of the SCHUNK SVH Driver suite.
//
// This program is free software licensed under the LGPL
// (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
// You can find a copy of this license in LICENSE folder in the top
// directory of the source code.
//
// © Copyright 2017 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
// © Copyright 2017 FZI Forschungszentrum Informatik, Karlsruhe, Germany
//
// -- END LICENSE BLOCK ------------------------------------------------

//----------------------------------------------------------------------
//----------------------------------------------------------------------



#include <DynamicParameterClass.h>

/*
 * The DynamicParameter class reads out the parameter file and searches for the proposed version!
 */

DynamicParameter::DynamicParameter(const uint16_t major_version,
                                   const uint16_t minor_version,
                                   XmlRpc::XmlRpcValue& parameters)
{
  // joint names used for parameter mapping from string to SVHChannel
  // currently hardcoded...
  m_name_to_enum[driver_svh::eSVH_THUMB_FLEXION]          = "THUMB_FLEXION";
  m_name_to_enum[driver_svh::eSVH_THUMB_OPPOSITION]       = "THUMB_OPPOSITION";
  m_name_to_enum[driver_svh::eSVH_INDEX_FINGER_DISTAL]    = "INDEX_FINGER_DISTAL";
  m_name_to_enum[driver_svh::eSVH_INDEX_FINGER_PROXIMAL]  = "INDEX_FINGER_PROXIMAL";
  m_name_to_enum[driver_svh::eSVH_MIDDLE_FINGER_DISTAL]   = "MIDDLE_FINGER_DISTAL";
  m_name_to_enum[driver_svh::eSVH_MIDDLE_FINGER_PROXIMAL] = "MIDDLE_FINGER_PROXIMAL";
  m_name_to_enum[driver_svh::eSVH_RING_FINGER]            = "RING_FINGER";
  m_name_to_enum[driver_svh::eSVH_PINKY]                  = "PINKY";
  m_name_to_enum[driver_svh::eSVH_FINGER_SPREAD]          = "FINGER_SPREAD";

  // Assertion to test whether the vector sizes are the same. Otherwise fail
  ROS_ASSERT(static_cast<driver_svh::SVHChannel>(m_name_to_enum.size()) ==
             driver_svh::eSVH_DIMENSION);

  try
  {
    parse_parameters(major_version, minor_version, parameters);
  }
  catch (XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR_STREAM("parsing error: " << e.getMessage() << "! Error code: " << e.getCode());
    ROS_ERROR("ATTENTION: YOU HAVE AN INCORRECT PARAMETER FILE!!!");
    exit(0);
  }

}

/*
 *  This function converts a RpcValue Vector to a std::vector
 */

bool DynamicParameter::xml_rpc_value_to_vector(XmlRpc::XmlRpcValue my_array,
                                               std::vector<float>& my_vector)
{
  my_vector.clear();

  for (size_t i = 0; i < (unsigned) my_array.size(); ++i)
  {
    ROS_ASSERT(my_array[i].getType() == XmlRpc::XmlRpcValue::TypeDouble ||
               my_array[i].getType() == XmlRpc::XmlRpcValue::TypeInt);

    if (my_array[i].getType() == XmlRpc::XmlRpcValue::TypeDouble)
    {
      my_vector.push_back(static_cast<double>(my_array[i]));
    }
    else if (my_array[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
    {
      int value = my_array[i];
      my_vector.push_back(static_cast<double>(value));
    }
  }

  return true;
}

/*
 * This function parses the parameter file and set the corresponding major and minor version
 */

void DynamicParameter::parse_parameters(const uint16_t major_version_target,
                                const uint16_t minor_version_target,
                                XmlRpc::XmlRpcValue& parameters)
{
  ROS_INFO("Trying to search controller parameters for version: %d.%d", major_version_target, minor_version_target);

  m_settings.major_version = 0;
  m_settings.minor_version = 0;

  if ( 0 == major_version_target || 5 < major_version_target )
  {
    ROS_ERROR("Could not establish connection to Schunk SVH. Please check connections and power source!");
    return;
  }

  if (0 < parameters.size())
  {
    ROS_DEBUG("There exist %d different parameter versions", parameters.size());
    for (size_t i = 0; i < (unsigned) parameters.size(); ++i)
    {
      XmlRpc::XmlRpcValue parameter_set_yaml = parameters[i]["parameter_set"];

      // the following parameters are for this version
      uint16_t major_version_read = int(parameter_set_yaml["major_version"]);
      uint16_t minor_version_read = int(parameter_set_yaml["minor_version"]);
      if (m_settings.major_version > major_version_read)
      {
        ROS_DEBUG("Skipping version %d.%d as better matching version was already found",
                  major_version_read,
                  minor_version_read
        );
        continue;
      }

      bool is_correct_version    = major_version_read == major_version_target
                                && minor_version_read == minor_version_target;
      bool is_same_major_version = major_version_read == major_version_target
                                && minor_version_read <= minor_version_target;
      bool is_default_state      = major_version_read == 0
                                && minor_version_read == 0;

      if (is_correct_version || is_same_major_version || is_default_state)
      {
        ROS_DEBUG("major version: %d minor version: %d", major_version_read, minor_version_read);

        for (std::map<driver_svh::SVHChannel, std::string>::iterator it = m_name_to_enum.begin();
              it != m_name_to_enum.end();
              ++it)
        {
          m_settings.major_version = major_version_read;
          m_settings.minor_version = minor_version_read;
          if (parameter_set_yaml.hasMember(it->second))
          {
            ROS_DEBUG("Parameter Name: %s", it->second.c_str());

            m_settings.position_settings_given[it->first] = xml_rpc_value_to_vector(
              parameter_set_yaml[it->second]["position_controller"], m_settings.position_settings[it->first]);
            m_settings.current_settings_given[it->first] = xml_rpc_value_to_vector(
              parameter_set_yaml[it->second]["current_controller"], m_settings.current_settings[it->first]);
            m_settings.home_settings_given[it->first] = xml_rpc_value_to_vector(
              parameter_set_yaml[it->second]["home_settings"], m_settings.home_settings[it->first]);
          }
          else
          {
            std::stringstream error_stream;
            error_stream << "Could not find parameters for channel " << it->second;
            throw(XmlRpc::XmlRpcException(error_stream.str(), -1));
          }
        }

        // First Reading of parameters
        if (is_default_state)
        {
          is_default_state = false;
        }
        else if (is_correct_version)
        {
          ROS_INFO("Did find correct version");
          return;
        }
        else
        {
          ROS_DEBUG("Did find same major version");
        }
      }
    }
  }
  ROS_WARN_STREAM("DID NOT FIND EXACT VERSION: " << major_version_target << "."
                                                 << minor_version_target
                                                 << " Falling back to: "
                                                 << m_settings.major_version
                                                 << "."
                                                 << m_settings.minor_version);
}