generic_tactiles.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/generic_tactiles.hpp"
00029 #include <sr_utilities/sr_math_utils.hpp>
00030 #include <cctype>
00031 
00032 //NOTE: The length used in this generic tactile class (that is used to obtain common information to determine the actual type of tactile sensors)
00033 // should be the the minimum length of all the existing types of tactile sensors
00034 // (this is used only to sanitise_string for certain data types)
00035 #define TACTILE_DATA_LENGTH_BYTES TACTILE_DATA_LENGTH_BYTES_v1
00036 
00037 namespace tactiles
00038 {
00039   template <class StatusType, class CommandType>
00040   const unsigned int GenericTactiles<StatusType, CommandType>::nb_tactiles = 5;
00041 
00042   template <class StatusType, class CommandType>
00043   GenericTactiles<StatusType, CommandType>::GenericTactiles(std::vector<generic_updater::UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00044   {
00045     sensor_updater = boost::shared_ptr<generic_updater::SensorUpdater<CommandType> >(new generic_updater::SensorUpdater<CommandType>(update_configs_vector, update_state));
00046     if(update_state != operation_mode::device_update_state::INITIALIZATION)
00047     {
00048       reset_service_client_ = nodehandle_.advertiseService("tactiles/reset", &GenericTactiles::reset, this);
00049     }
00050 
00051     //initialize the vector of tactiles
00052     tactiles_vector = boost::shared_ptr< std::vector<GenericTactileData> >( new std::vector<GenericTactileData>(nb_tactiles) );
00053     all_tactile_data = boost::shared_ptr<std::vector<AllTactileData> >( new std::vector<AllTactileData>(nb_tactiles) );
00054 
00055     for(unsigned int i=0;i<nb_tactiles;i++)
00056     {
00057       GenericTactileData tmp_pst;
00058       tactiles_vector->push_back( tmp_pst );
00059     }
00060   }
00061 
00062   template <class StatusType, class CommandType>
00063   void GenericTactiles<StatusType, CommandType>::update(StatusType* status_data)
00064   {
00065     int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
00066     //TODO: use memcopy instead?
00067     for( unsigned int id_sensor = 0; id_sensor < nb_tactiles; ++id_sensor)
00068     {
00069       ROS_DEBUG_STREAM(" received: " << static_cast<int32u>(status_data->tactile_data_type));
00070 
00071       switch( static_cast<int32u>(status_data->tactile_data_type) )
00072       {
00073       //COMMON DATA
00074       case TACTILE_SENSOR_TYPE_WHICH_SENSORS:
00075         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00076         {
00077           if( tactiles_vector != NULL )
00078             tactiles_vector->at(id_sensor).which_sensor = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00079           ROS_DEBUG_STREAM(" tact["<<id_sensor<<"] = " << tactiles_vector->at(id_sensor).which_sensor);
00080 
00081         }
00082         break;
00083 
00084       case TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ:
00085         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00086         {
00087           if( tactiles_vector != NULL )
00088             tactiles_vector->at(id_sensor).sample_frequency = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00089         }
00090         break;
00091 
00092       case TACTILE_SENSOR_TYPE_MANUFACTURER:
00093       {
00094         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00095         {
00096           tactiles_vector->at(id_sensor).manufacturer = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00097         }
00098       }
00099       break;
00100 
00101       case TACTILE_SENSOR_TYPE_SERIAL_NUMBER:
00102       {
00103         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00104         {
00105           tactiles_vector->at(id_sensor).serial_number = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00106         }
00107       }
00108       break;
00109 
00110       case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
00111         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00112         {
00113           if( tactiles_vector != NULL )
00114             tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
00115         }
00116         break;
00117 
00118       case TACTILE_SENSOR_TYPE_PCB_VERSION:
00119         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00120         {
00121           if( tactiles_vector != NULL )
00122             tactiles_vector->at(id_sensor).pcb_version = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00123         }
00124         break;
00125 
00126       default:
00127         break;
00128 
00129       } //end switch
00130     } //end for tactile
00131 
00132     if(sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
00133     {
00134       process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
00135       if(sensor_updater->initialization_configs_vector.size() == 0)
00136         sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
00137     }
00138   }
00139 
00140   template <class StatusType, class CommandType>
00141   void GenericTactiles<StatusType, CommandType>::process_received_data_type(int32u data)
00142   {
00143     unsigned int i;
00144     for(i=0; i<sensor_updater->initialization_configs_vector.size(); i++)
00145     {
00146       if (sensor_updater->initialization_configs_vector[i].what_to_update == data) break;
00147     }
00148     if(i<sensor_updater->initialization_configs_vector.size())
00149       sensor_updater->initialization_configs_vector.erase(sensor_updater->initialization_configs_vector.begin() + i);
00150   }
00151 
00152   template <class StatusType, class CommandType>
00153   void GenericTactiles<StatusType, CommandType>::publish()
00154   {
00155     //We don't publish anything during the initialization phase
00156   }//end publish
00157 
00158   template <class StatusType, class CommandType>
00159   void GenericTactiles<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00160                                         diagnostic_updater::DiagnosticStatusWrapper &d)
00161   {
00162     //We don't publish diagnostics during the initialization phase
00163   }
00164 
00173   template <class StatusType, class CommandType>
00174   bool GenericTactiles<StatusType, CommandType>::reset(std_srvs::Empty::Request& request,
00175                               std_srvs::Empty::Response& response)
00176   {
00177     ROS_INFO_STREAM("Resetting tactiles");
00178 
00179     return sensor_updater->reset();
00180   }
00181 
00182   template <class StatusType, class CommandType>
00183   std::string GenericTactiles<StatusType, CommandType>::sanitise_string( const char* raw_string, const unsigned int str_size )
00184   {
00185     std::string sanitised_string = "";
00186     for (unsigned int i = 0; i < str_size; ++i)
00187     {
00188       char tmp = static_cast<char>( raw_string[i] );
00189       if( tmp != 0 )
00190       {
00191         if( tmp >= '\x20' && tmp <= '\x7E')
00192         {
00193           sanitised_string += static_cast<char>( raw_string[i] ) ;
00194         }
00195         else
00196           sanitised_string += '?';
00197       }
00198       else
00199         break;
00200     }
00201     return sanitised_string;
00202   }
00203 
00204   template <class StatusType, class CommandType>
00205   std::vector<AllTactileData>* GenericTactiles<StatusType, CommandType>::get_tactile_data()
00206   {
00207     return all_tactile_data.get();
00208   }
00209 
00210   //Only to ensure that the template class is compiled for the types we are interested in
00211   template class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00212   template class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00213   template class GenericTactiles<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00214 }
00215 
00216 /* For the emacs weenies in the crowd.
00217    Local Variables:
00218    c-basic-offset: 2
00219    End:
00220 */
00221 
00222 


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:37:50