generic_tactiles.cpp

Go to the documentation of this file.

#include "sr_robot_lib/generic_tactiles.hpp"
#include <sr_utilities/sr_math_utils.hpp>
#include <cctype>

//NOTE: The length used in this generic tactile class (that is used to obtain common information to determine the actual type of tactile sensors)
// should be the the minimum length of all the existing types of tactile sensors
// (this is used only to sanitise_string for certain data types)
#define TACTILE_DATA_LENGTH_BYTES TACTILE_DATA_LENGTH_BYTES_v1

namespace tactiles
{
  template <class StatusType, class CommandType>
  const unsigned int GenericTactiles<StatusType, CommandType>::nb_tactiles = 5;

  template <class StatusType, class CommandType>
  GenericTactiles<StatusType, CommandType>::GenericTactiles(std::vector<generic_updater::UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
  {
    sensor_updater = boost::shared_ptr<generic_updater::SensorUpdater<CommandType> >(new generic_updater::SensorUpdater<CommandType>(update_configs_vector, update_state));
    if(update_state != operation_mode::device_update_state::INITIALIZATION)
    {
      reset_service_client_ = nodehandle_.advertiseService("tactiles/reset", &GenericTactiles::reset, this);
    }

    //initialize the vector of tactiles
    tactiles_vector = boost::shared_ptr< std::vector<GenericTactileData> >( new std::vector<GenericTactileData>(nb_tactiles) );
    all_tactile_data = boost::shared_ptr<std::vector<AllTactileData> >( new std::vector<AllTactileData>(nb_tactiles) );

    for(unsigned int i=0;i<nb_tactiles;i++)
    {
      GenericTactileData tmp_pst;
      tactiles_vector->push_back( tmp_pst );
    }
  }

  template <class StatusType, class CommandType>
  void GenericTactiles<StatusType, CommandType>::update(StatusType* status_data)
  {
    int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
    //TODO: use memcopy instead?
    for( unsigned int id_sensor = 0; id_sensor < nb_tactiles; ++id_sensor)
    {
      ROS_DEBUG_STREAM(" received: " << static_cast<int32u>(status_data->tactile_data_type));

      switch( static_cast<int32u>(status_data->tactile_data_type) )
      {
      //COMMON DATA
      case TACTILE_SENSOR_TYPE_WHICH_SENSORS:
        if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
        {
          if( tactiles_vector != NULL )
            tactiles_vector->at(id_sensor).which_sensor = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
          ROS_DEBUG_STREAM(" tact["<<id_sensor<<"] = " << tactiles_vector->at(id_sensor).which_sensor);

        }
        break;

      case TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ:
        if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
        {
          if( tactiles_vector != NULL )
            tactiles_vector->at(id_sensor).sample_frequency = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
        }
        break;

      case TACTILE_SENSOR_TYPE_MANUFACTURER:
      {
        if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
        {
          tactiles_vector->at(id_sensor).manufacturer = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
        }
      }
      break;

      case TACTILE_SENSOR_TYPE_SERIAL_NUMBER:
      {
        if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
        {
          tactiles_vector->at(id_sensor).serial_number = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
        }
      }
      break;

      case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
        if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
        {
          if( tactiles_vector != NULL )
            tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
        }
        break;

      case TACTILE_SENSOR_TYPE_PCB_VERSION:
        if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
        {
          if( tactiles_vector != NULL )
            tactiles_vector->at(id_sensor).pcb_version = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
        }
        break;

      default:
        break;

      } //end switch
    } //end for tactile

    if(sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
    {
      process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
      if(sensor_updater->initialization_configs_vector.size() == 0)
        sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
    }
  }

  template <class StatusType, class CommandType>
  void GenericTactiles<StatusType, CommandType>::process_received_data_type(int32u data)
  {
    unsigned int i;
    for(i=0; i<sensor_updater->initialization_configs_vector.size(); i++)
    {
      if (sensor_updater->initialization_configs_vector[i].what_to_update == data) break;
    }
    if(i<sensor_updater->initialization_configs_vector.size())
      sensor_updater->initialization_configs_vector.erase(sensor_updater->initialization_configs_vector.begin() + i);
  }

  template <class StatusType, class CommandType>
  void GenericTactiles<StatusType, CommandType>::publish()
  {
    //We don't publish anything during the initialization phase
  }//end publish

  template <class StatusType, class CommandType>
  void GenericTactiles<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
                                        diagnostic_updater::DiagnosticStatusWrapper &d)
  {
    //We don't publish diagnostics during the initialization phase
  }

  template <class StatusType, class CommandType>
  bool GenericTactiles<StatusType, CommandType>::reset(std_srvs::Empty::Request& request,
                              std_srvs::Empty::Response& response)
  {
    ROS_INFO_STREAM("Resetting tactiles");

    return sensor_updater->reset();
  }

  template <class StatusType, class CommandType>
  std::string GenericTactiles<StatusType, CommandType>::sanitise_string( const char* raw_string, const unsigned int str_size )
  {
    std::string sanitised_string = "";
    for (unsigned int i = 0; i < str_size; ++i)
    {
      char tmp = static_cast<char>( raw_string[i] );
      if( tmp != 0 )
      {
        if( tmp >= '\x20' && tmp <= '\x7E')
        {
          sanitised_string += static_cast<char>( raw_string[i] ) ;
        }
        else
          sanitised_string += '?';
      }
      else
        break;
    }
    return sanitised_string;
  }

  template <class StatusType, class CommandType>
  std::vector<AllTactileData>* GenericTactiles<StatusType, CommandType>::get_tactile_data()
  {
    return all_tactile_data.get();
  }

  //Only to ensure that the template class is compiled for the types we are interested in
  template class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
  template class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
  template class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
}

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