sr_robot_lib.cpp

Go to the documentation of this file.

#include "sr_robot_lib/sr_robot_lib.hpp"
#include <string>
#include <utility>
#include <map>
#include <vector>
#include <boost/foreach.hpp>

#include <sys/time.h>

#include <ros/ros.h>

#include "sr_robot_lib/shadow_PSTs.hpp"
#include "sr_robot_lib/biotac.hpp"
#include "sr_robot_lib/UBI0.hpp"
#include <controller_manager_msgs/ListControllers.h>
#include <sr_robot_lib/motor_updater.hpp>

#define SERIOUS_ERROR_FLAGS PALM_0200_EDC_SERIOUS_ERROR_FLAGS
#define error_flag_names palm_0200_edc_error_flag_names

using std::vector;
using std::string;
using std::pair;
using std::map;
using std::ostringstream;
using sr_actuator::SrMotorActuator;
using tactiles::GenericTactiles;
using tactiles::ShadowPSTs;
using tactiles::Biotac;
using tactiles::UBI0;
using ros_ethercat_model::RobotState;
using generic_updater::MotorUpdater;
using shadow_joints::CalibrationMap;
using shadow_joints::Joint;
using shadow_joints::JointToSensor;
using shadow_joints::MotorWrapper;
using shadow_joints::PartialJointToSensor;
using generic_updater::MotorUpdater;
using generic_updater::UpdateConfig;
using boost::shared_ptr;
using boost::ptr_vector;


namespace shadow_robot
{
#ifdef DEBUG_PUBLISHER
  // max of 20 publishers for debug
  template<class StatusType, class CommandType>
  const int SrRobotLib<StatusType, CommandType>::nb_debug_publishers_const = 20;
  // template <class StatusType, class CommandType>
  // const int SrRobotLib<StatusType, CommandType>::debug_mutex_lock_wait_time = 100;
#endif

  template <class StatusType, class CommandType>
  const int SrRobotLib<StatusType, CommandType>::nb_sensor_data = 37;

  template <class StatusType, class CommandType>
  const char *SrRobotLib<StatusType, CommandType>::human_readable_sensor_data_types[nb_sensor_data] =
      {
          "TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ",
          "TACTILE_SENSOR_TYPE_MANUFACTURER",
          "TACTILE_SENSOR_TYPE_SERIAL_NUMBER",
          "TACTILE_SENSOR_TYPE_SOFTWARE_VERSION",
          "TACTILE_SENSOR_TYPE_PCB_VERSION",
          "TACTILE_SENSOR_TYPE_WHICH_SENSORS",
          "TACTILE_SENSOR_TYPE_PST3_PRESSURE_TEMPERATURE",
          "TACTILE_SENSOR_TYPE_PST3_PRESSURE_RAW_ZERO_TRACKING",
          "TACTILE_SENSOR_TYPE_PST3_DAC_VALUE",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_20",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_21",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_22",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_23",
          "TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_24",
          "TACTILE_SENSOR_TYPE_BIOTAC_PDC",
          "TACTILE_SENSOR_TYPE_BIOTAC_TAC",
          "TACTILE_SENSOR_TYPE_BIOTAC_TDC",
          "TACTILE_SENSOR_TYPE_UBI0_TACTILE"
        };

  template <class StatusType, class CommandType>
  const int32u SrRobotLib<StatusType, CommandType>::sensor_data_types[nb_sensor_data] =
  {
          TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ,
          TACTILE_SENSOR_TYPE_MANUFACTURER,
          TACTILE_SENSOR_TYPE_SERIAL_NUMBER,
          TACTILE_SENSOR_TYPE_SOFTWARE_VERSION,
          TACTILE_SENSOR_TYPE_PCB_VERSION,
          TACTILE_SENSOR_TYPE_WHICH_SENSORS,
          TACTILE_SENSOR_TYPE_PST3_PRESSURE_TEMPERATURE,
          TACTILE_SENSOR_TYPE_PST3_PRESSURE_RAW_ZERO_TRACKING,
          TACTILE_SENSOR_TYPE_PST3_DAC_VALUE,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_20,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_21,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_22,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_23,
          TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_24,
          TACTILE_SENSOR_TYPE_BIOTAC_PDC,
          TACTILE_SENSOR_TYPE_BIOTAC_TAC,
          TACTILE_SENSOR_TYPE_BIOTAC_TDC,
          TACTILE_SENSOR_TYPE_UBI0_TACTILE
    };

  template<class StatusType, class CommandType>
  const double SrRobotLib<StatusType, CommandType>::tactile_timeout = 10.0;

  template<class StatusType, class CommandType>
  SrRobotLib<StatusType, CommandType>::SrRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh,
                                                  ros::NodeHandle nhtilde, string device_id, string joint_prefix)
          : main_pic_idle_time(0),
            main_pic_idle_time_min(1000),
            tactile_current_state(operation_mode::device_update_state::INITIALIZATION),
            hw_(static_cast<RobotState *> (hw)),
            device_id_(device_id),
            joint_prefix_(joint_prefix),
            nullify_demand_(false),
            nodehandle_(nh),
            nh_tilde(nhtilde),

            // advertise the service to nullify the demand sent to the motor
            // this makes it possible to easily stop the controllers.
            nullify_demand_server_(
                    nh_tilde.advertiseService("nullify_demand", &SrRobotLib::nullify_demand_callback, this)),

            // initialises self tests (false as this is not a simulated hand\)
            // self_tests_(new SrSelfTest(false)),
            //  self_test_thread_(new boost::thread(boost::bind(&SrRobotLib::checkSelfTests, this))),

            // read the generic sensor polling frequency from the parameter server
            generic_sensor_update_rate_configs_vector(
                    read_update_rate_configs("generic_sensor_data_update_rate/", nb_sensor_data,
                                             human_readable_sensor_data_types, sensor_data_types)),

            // read the pst3 sensor polling frequency from the parameter server
            pst3_sensor_update_rate_configs_vector(
                    read_update_rate_configs("pst3_sensor_data_update_rate/", nb_sensor_data,
                                             human_readable_sensor_data_types, sensor_data_types)),

            // read the biotac sensor polling frequency from the parameter server
            biotac_sensor_update_rate_configs_vector(
                    read_update_rate_configs("biotac_sensor_data_update_rate/", nb_sensor_data,
                                             human_readable_sensor_data_types, sensor_data_types)),

            // read the UBI0 sensor polling frequency from the parameter server
            ubi0_sensor_update_rate_configs_vector(
                    read_update_rate_configs("ubi0_sensor_data_update_rate/", nb_sensor_data,
                                             human_readable_sensor_data_types, sensor_data_types)),

            tactile_init_max_duration(tactile_timeout),

            // Create a one-shot timer
            tactile_check_init_timeout_timer(
                    this->nh_tilde.createTimer(tactile_init_max_duration,
                                               boost::bind(
                                                       &SrRobotLib<StatusType,
                                                               CommandType>::tactile_init_timer_callback,
                                                       this, _1), true)),
            lock_tactile_init_timeout_(boost::shared_ptr<boost::mutex>(new boost::mutex())),
            tactiles_init(shared_ptr<GenericTactiles<StatusType, CommandType> >(
                    new GenericTactiles<StatusType, CommandType>(nodehandle_, device_id_,
                                                                 generic_sensor_update_rate_configs_vector,
                                                                 operation_mode::device_update_state::INITIALIZATION))),

            // initialize the calibration map
            calibration_map(read_joint_calibration()),
            // initialize the coupled joints calibration map
            coupled_calibration_map(read_coupled_joint_calibration())
  {
      std::vector<std::string> calibration_map_keys = calibration_map.keys();
      for (auto & coupled_calibration : coupled_calibration_map)
      {
        if (std::find(calibration_map_keys.begin(),
              calibration_map_keys.end(), coupled_calibration.first)
                != calibration_map_keys.end())
        {
          ROS_WARN_STREAM("Calibration for joint " << coupled_calibration.first << " present in both regular and "
                          "coupled form in the calibration file. Only coupled calibration will be used!");
        }
      }
  }

  template<class StatusType, class CommandType>
  void SrRobotLib<StatusType, CommandType>::reinitialize_sensors()
  {
    // Create a new GenericTactiles object
    tactiles_init = shared_ptr<GenericTactiles<StatusType, CommandType> >(
            new GenericTactiles<StatusType, CommandType>(nodehandle_, device_id_,
                                                         generic_sensor_update_rate_configs_vector,
                                                         operation_mode::device_update_state::INITIALIZATION));
    tactile_current_state = operation_mode::device_update_state::INITIALIZATION;
  }

  template<class StatusType, class CommandType>
  bool SrRobotLib<StatusType, CommandType>::nullify_demand_callback(sr_robot_msgs::NullifyDemand::Request &request,
                                                                    sr_robot_msgs::NullifyDemand::Response &response)
  {
    if (request.nullify_demand)
      ROS_INFO_STREAM(
              "Nullifying the demand sent to the motor. "
                      "Will ignore the values computed by the controllers and send 0.");
    else
      ROS_INFO_STREAM("Using the value computed by the controllers to send the demands to the motors.");

    nullify_demand_ = request.nullify_demand;
    return true;
  }

  template<class StatusType, class CommandType>
  void SrRobotLib<StatusType, CommandType>::build_tactile_command(CommandType *command)
  {
    // Mutual exclusion with the the initialization timeout
    boost::mutex::scoped_lock l(*lock_tactile_init_timeout_);

    if (tactile_current_state == operation_mode::device_update_state::INITIALIZATION)
    {
      if (tactiles_init->sensor_updater->build_init_command(command)
          != operation_mode::device_update_state::INITIALIZATION)
      {
        tactile_current_state = operation_mode::device_update_state::OPERATION;

        tactile_check_init_timeout_timer.stop();

        switch (tactiles_init->tactiles_vector->at(0).which_sensor)
        {
          case TACTILE_SENSOR_PROTOCOL_TYPE_PST3:
            tactiles = shared_ptr<ShadowPSTs<StatusType, CommandType> >(
                    new ShadowPSTs<StatusType, CommandType>(nodehandle_, device_id_,
                                                            pst3_sensor_update_rate_configs_vector,
                                                            operation_mode::device_update_state::OPERATION,
                                                            tactiles_init->tactiles_vector));

            ROS_INFO("PST3 tactiles initialized");
            break;

          case TACTILE_SENSOR_PROTOCOL_TYPE_BIOTAC_2_3:
            tactiles = shared_ptr<Biotac<StatusType, CommandType> >(
                    new Biotac<StatusType, CommandType>(nodehandle_, device_id_,
                                                        biotac_sensor_update_rate_configs_vector,
                                                        operation_mode::device_update_state::OPERATION,
                                                        tactiles_init->tactiles_vector));

            ROS_INFO("Biotac tactiles initialized");
            break;

          case TACTILE_SENSOR_PROTOCOL_TYPE_UBI0:
            tactiles = shared_ptr<UBI0<StatusType, CommandType> >(
                    new UBI0<StatusType, CommandType>(nodehandle_, device_id_, ubi0_sensor_update_rate_configs_vector,
                                                      operation_mode::device_update_state::OPERATION,
                                                      tactiles_init->tactiles_vector));

            ROS_INFO("UBI0 tactiles initialized");
            break;

          case TACTILE_SENSOR_PROTOCOL_TYPE_INVALID:
            ROS_WARN_STREAM("TACTILE_SENSOR_PROTOCOL_TYPE_INVALID!!");
            break;
          case TACTILE_SENSOR_PROTOCOL_TYPE_CONFLICTING:
            ROS_WARN_STREAM("TACTILE_SENSOR_PROTOCOL_TYPE_CONFLICTING!!");
            break;
        }
      }
    }
    else
    {
      tactile_current_state = tactiles->sensor_updater->build_command(command);
    }
  }

  template<class StatusType, class CommandType>
  void SrRobotLib<StatusType, CommandType>::update_tactile_info(StatusType *status)
  {
    // Mutual exclusion with the the initialization timeout
    boost::mutex::scoped_lock l(*lock_tactile_init_timeout_);
    if (tactile_current_state == operation_mode::device_update_state::INITIALIZATION)
    {
      if (tactiles_init != NULL)
      {
        tactiles_init->update(status);
      }
    }
    else
    {
      if (tactiles != NULL)
      {
        tactiles->update(status);
      }
    }
  }

  template<class StatusType, class CommandType>
  CalibrationMap SrRobotLib<StatusType, CommandType>::read_joint_calibration()
  {
    CalibrationMap joint_calibration;

    XmlRpc::XmlRpcValue calib;
    nodehandle_.getParam("sr_calibrations", calib);
    ROS_ASSERT(calib.getType() == XmlRpc::XmlRpcValue::TypeArray);
    // iterate on all the joints
    for (int32_t index_cal = 0; index_cal < calib.size(); ++index_cal)
    {
      // check the calibration is well formatted:
      // first joint name, then calibration table
      ROS_ASSERT(calib[index_cal][0].getType() == XmlRpc::XmlRpcValue::TypeString);
      ROS_ASSERT(calib[index_cal][1].getType() == XmlRpc::XmlRpcValue::TypeArray);

      string joint_name = static_cast<string> (calib[index_cal][0]);
      vector<joint_calibration::Point> calib_table_tmp;

      // now iterates on the calibration table for the current joint
      for (int32_t index_table = 0; index_table < calib[index_cal][1].size(); ++index_table)
      {
        ROS_ASSERT(calib[index_cal][1][index_table].getType() == XmlRpc::XmlRpcValue::TypeArray);
        // only 2 values per calibration point: raw and calibrated (doubles)
        ROS_ASSERT(calib[index_cal][1][index_table].size() == 2);
        ROS_ASSERT(calib[index_cal][1][index_table][0].getType() == XmlRpc::XmlRpcValue::TypeDouble);
        ROS_ASSERT(calib[index_cal][1][index_table][1].getType() == XmlRpc::XmlRpcValue::TypeDouble);

        joint_calibration::Point point_tmp;
        point_tmp.raw_value = static_cast<double> (calib[index_cal][1][index_table][0]);
        point_tmp.calibrated_value = sr_math_utils::to_rad(static_cast<double> (calib[index_cal][1][index_table][1]));
        calib_table_tmp.push_back(point_tmp);
      }

      joint_calibration.insert(joint_name, shared_ptr<shadow_robot::JointCalibration>(
              new shadow_robot::JointCalibration(calib_table_tmp)));
    }

    return joint_calibration;
  }  // end read_joint_calibration

  template<class StatusType, class CommandType>
  typename SrRobotLib<StatusType, CommandType>::CoupledJointMapType
    SrRobotLib<StatusType, CommandType>::read_coupled_joint_calibration()
  {
    CoupledJointMapType coupled_joint_calibration;
    XmlRpc::XmlRpcValue calib;
    nodehandle_.getParam("sr_calibrations_coupled", calib);
    if (!calib.valid())
    {
      ROS_WARN("No coupled calibration specified.");
      return coupled_joint_calibration;
    }

    ROS_ASSERT(XmlRpc::XmlRpcValue::TypeArray == calib.getType());
    // iterate on all the joint pairs
    for (int32_t cal_index = 0; cal_index < calib.size(); ++cal_index)
    {
      std::vector<std::string> joint_pair;
      std::vector<double> raw_values_coupled_0, raw_values_coupled_1, calibrated_values_0, calibrated_values_1;

      ROS_ASSERT(XmlRpc::XmlRpcValue::TypeArray == calib[cal_index][0].getType());
      ROS_ASSERT(2 == calib[cal_index][0].size());
      ROS_ASSERT(XmlRpc::XmlRpcValue::TypeArray == calib[cal_index][1].getType());

      // check if values format is ok
      for (int32_t raw_and_calibrated_value_index = 0;
            raw_and_calibrated_value_index < calib[cal_index][1].size();
              ++raw_and_calibrated_value_index)
      {
        ROS_ASSERT(XmlRpc::XmlRpcValue::TypeArray == calib[cal_index][1][raw_and_calibrated_value_index].getType());
        ROS_ASSERT(3 == calib[cal_index][1][raw_and_calibrated_value_index].size());

        ROS_ASSERT(XmlRpc::XmlRpcValue::TypeArray == calib[cal_index][1][raw_and_calibrated_value_index][0].getType());
        ROS_ASSERT(2 == calib[cal_index][1][raw_and_calibrated_value_index][0].size());
        ROS_ASSERT(XmlRpc::XmlRpcValue::TypeDouble ==
                    calib[cal_index][1][raw_and_calibrated_value_index][0][0].getType());
        ROS_ASSERT(XmlRpc::XmlRpcValue::TypeDouble ==
                    calib[cal_index][1][raw_and_calibrated_value_index][0][1].getType());
        ROS_ASSERT(XmlRpc::XmlRpcValue::TypeDouble ==
                    calib[cal_index][1][raw_and_calibrated_value_index][1].getType());
        ROS_ASSERT(XmlRpc::XmlRpcValue::TypeDouble ==
                    calib[cal_index][1][raw_and_calibrated_value_index][2].getType());

        raw_values_coupled_0.push_back(static_cast<double>(calib[cal_index][1][raw_and_calibrated_value_index][0][0]));
        raw_values_coupled_0.push_back(static_cast<double>(calib[cal_index][1][raw_and_calibrated_value_index][0][1]));
        raw_values_coupled_1.push_back(static_cast<double>(calib[cal_index][1][raw_and_calibrated_value_index][0][1]));
        raw_values_coupled_1.push_back(static_cast<double>(calib[cal_index][1][raw_and_calibrated_value_index][0][0]));
        calibrated_values_0.push_back(sr_math_utils::to_rad(static_cast<double>
                                                            (calib[cal_index][1][raw_and_calibrated_value_index][1])));
        calibrated_values_1.push_back(sr_math_utils::to_rad(static_cast<double>
                                                            (calib[cal_index][1][raw_and_calibrated_value_index][2])));
      }  //  value format ok

      ROS_ASSERT(XmlRpc::XmlRpcValue::TypeString == calib[cal_index][0][0].getType());
      ROS_ASSERT(XmlRpc::XmlRpcValue::TypeString == calib[cal_index][0][1].getType());

      std::string joint_0_name = static_cast<string>(calib[cal_index][0][0]);
      std::string joint_1_name = static_cast<string>(calib[cal_index][0][1]);

      CoupledJoint coupled_joint_0(joint_0_name, joint_1_name, raw_values_coupled_0, calibrated_values_0);
      CoupledJoint coupled_joint_1(joint_1_name, joint_0_name, raw_values_coupled_1, calibrated_values_1);

      coupled_joint_calibration.insert(std::pair<std::string, CoupledJoint>(joint_0_name, coupled_joint_0));
      coupled_joint_calibration.insert(std::pair<std::string, CoupledJoint>(joint_1_name, coupled_joint_1));
    }
    return coupled_joint_calibration;
  }  // end read_coupled_joint_calibration

  template<class StatusType, class CommandType>
  vector<JointToSensor> SrRobotLib<StatusType, CommandType>::read_joint_to_sensor_mapping()
  {
    vector<JointToSensor> joint_to_sensor_vect;

    map<string, int> sensors_map;
    for (unsigned int i = 0; i < SENSORS_NUM_0220; ++i)
    {
      sensors_map[sensor_names[i]] = i;
    }

    XmlRpc::XmlRpcValue joint_to_sensor_mapping;
    nodehandle_.getParam("joint_to_sensor_mapping", joint_to_sensor_mapping);
    ROS_ASSERT(joint_to_sensor_mapping.getType() == XmlRpc::XmlRpcValue::TypeArray);
    for (int32_t i = 0; i < joint_to_sensor_mapping.size(); ++i)
    {
      JointToSensor tmp_vect;

      XmlRpc::XmlRpcValue map_one_joint = joint_to_sensor_mapping[i];

      // The parameter can either start by an array (sensor_name, coeff)
      // or by an integer to specify if we calibrate before combining
      // the different sensors
      int param_index = 0;
      // Check if the calibrate after combine int is set to 1
      if (map_one_joint[param_index].getType() == XmlRpc::XmlRpcValue::TypeInt)
      {
        if (1 == static_cast<int> (map_one_joint[0]))
        {
          tmp_vect.calibrate_after_combining_sensors = true;
        }
        else
        {
          tmp_vect.calibrate_after_combining_sensors = false;
        }

        param_index++;
      }
      else
      {
        // by default we calibrate before combining the sensors
        tmp_vect.calibrate_after_combining_sensors = false;
      }

      ROS_ASSERT(map_one_joint.getType() == XmlRpc::XmlRpcValue::TypeArray);
      for (int32_t i = param_index; i < map_one_joint.size(); ++i)
      {
        ROS_ASSERT(map_one_joint[i].getType() == XmlRpc::XmlRpcValue::TypeArray);
        PartialJointToSensor tmp_joint_to_sensor;

        ROS_ASSERT(map_one_joint[i][0].getType() == XmlRpc::XmlRpcValue::TypeString);
        tmp_vect.sensor_names.push_back(static_cast<string> (map_one_joint[i][0]));
        tmp_joint_to_sensor.sensor_id = sensors_map[static_cast<string> (map_one_joint[i][0])];

        ROS_ASSERT(map_one_joint[i][1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
        tmp_joint_to_sensor.coeff = static_cast<double> (map_one_joint[i][1]);
        tmp_vect.joint_to_sensor_vector.push_back(tmp_joint_to_sensor);
      }
      joint_to_sensor_vect.push_back(tmp_vect);
    }

    return joint_to_sensor_vect;
  }  // end read_joint_to_sensor_mapping

  template<class StatusType, class CommandType>
  vector<UpdateConfig> SrRobotLib<StatusType,
          CommandType>::read_update_rate_configs(string base_param,
                                                 int nb_data_defined,
                                                 const char *human_readable_data_types[],
                                                 const int32u data_types[])
  {
    vector<UpdateConfig> update_rate_configs_vector;
    typedef pair<string, int32u> ConfPair;
    vector<ConfPair> config;

    for (int i = 0; i < nb_data_defined; ++i)
    {
      ConfPair tmp;

      ROS_DEBUG_STREAM(" read " << base_param << " config [" << i << "] = " << human_readable_data_types[i]);

      tmp.first = base_param + human_readable_data_types[i];
      tmp.second = data_types[i];
      config.push_back(tmp);
    }

    for (unsigned int i = 0; i < config.size(); ++i)
    {
      double rate;
      if (nodehandle_.getParam(config[i].first, rate))
      {
        UpdateConfig config_tmp;

        config_tmp.when_to_update = rate;
        config_tmp.what_to_update = config[i].second;
        update_rate_configs_vector.push_back(config_tmp);

        ROS_DEBUG_STREAM(
                " read " << base_param << " config [" << i << "] = " << "what: " << config_tmp.what_to_update <<
                " when: " << config_tmp.when_to_update);
      }
    }

    return update_rate_configs_vector;
  }

  template<class StatusType, class CommandType>
  void SrRobotLib<StatusType, CommandType>::tactile_init_timer_callback(const ros::TimerEvent &event)
  {
    // Mutual exclusion with the the initialization timeout
    boost::mutex::scoped_lock l(*lock_tactile_init_timeout_);

    if (tactile_current_state == operation_mode::device_update_state::INITIALIZATION)
    {
      tactile_current_state = operation_mode::device_update_state::OPERATION;
      tactiles = boost::shared_ptr<tactiles::UBI0<StatusType, CommandType> >(
              new tactiles::UBI0<StatusType, CommandType>(nodehandle_, device_id_,
                                                          ubi0_sensor_update_rate_configs_vector,
                                                          operation_mode::device_update_state::OPERATION,
                                                          tactiles_init->tactiles_vector));
      ROS_ERROR_STREAM("Tactile Initialization Timeout: considering UBI0 tactiles");
    }
  }

  CoupledJoint::CoupledJoint(std::string joint_name, std::string joint_sibling_name,
                             std::vector<double> raw_values_coupled_vector,
                             std::vector<double> calibrated_values_vector)
  {
    name_ = joint_name;
    sibling_name_ = joint_sibling_name;

    calibration_points_ = raw_values_coupled_vector.size() / 2;
    total_points_  = calibration_points_ + nb_surrounding_points_;

    for (int i = 0; i < 2*total_points_; i++)
    {
      raw_values_coupled_.push_back(0);
    }

    for (int i = 0; i < total_points_; i++)
    {
      calibrated_values_.push_back(0);
    }

    for (int i = 0; i < 3*2*total_points_; i++)
    {
      triangle_.push_back(0);
      element_neighbor_.push_back(0);
    }

    for (std::vector<double>::size_type i = 0; i != raw_values_coupled_vector.size(); i++)
    {
      raw_values_coupled_[i] = raw_values_coupled_vector[i];
    }

    for (std::vector<double>::size_type i = 0; i != calibrated_values_vector.size(); i++)
    {
      calibrated_values_[i] = calibrated_values_vector[i];
    }

    process_calibration_values();
  }

  CoupledJoint::~CoupledJoint()
  {
  }

  void CoupledJoint::process_calibration_values()
  {
    // Generate additional points around the actual calibration points
    add_surrounding_points(calibration_points_, &raw_values_coupled_[0],
                           &calibrated_values_[0], nb_surrounding_points_);
    //
    //  Set up the Delaunay triangulation.
    //
    r8tris2(total_points_, &raw_values_coupled_[0], element_num_, &triangle_[0], &element_neighbor_[0]);

    for (int j = 0; j < element_num_; j++)
    {
        for (int i = 0; i < 3; i++)
        {
            if ( 0 < element_neighbor_[i+j*3] )
            {
            element_neighbor_[i+j*3] = element_neighbor_[i+j*3] - 1;
            }
        }
    }
  }

// Only to ensure that the template class is compiled for the types we are interested in
  template
  class SrRobotLib<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;

  template
  class SrRobotLib<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;

  template
  class SrRobotLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>;

  template
  class SrRobotLib<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;

}  // namespace shadow_robot

/* For the emacs weenies in the crowd.
 Local Variables:
 c-basic-offset: 2
 End:
*/