sr_robot_lib: shadow_PSTs.cpp Source File

Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/shadow_PSTs.hpp"
00029 #include <sr_utilities/sr_math_utils.hpp>
00030 
00031 #define TACTILE_DATA_LENGTH_BYTES TACTILE_DATA_LENGTH_BYTES_v1
00032 
00033 namespace tactiles
00034 {
00035   template <class StatusType, class CommandType>
00036   ShadowPSTs<StatusType, CommandType>::ShadowPSTs(std::vector<generic_updater::UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00037     : GenericTactiles<StatusType, CommandType>(update_configs_vector, update_state)
00038   {
00039     init(update_configs_vector, update_state);
00040   }
00041 
00042   template <class StatusType, class CommandType>
00043   ShadowPSTs<StatusType, CommandType>::ShadowPSTs(std::vector<generic_updater::UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state, boost::shared_ptr< std::vector<GenericTactileData> > init_tactiles_vector)
00044     : GenericTactiles<StatusType, CommandType>(update_configs_vector, update_state)
00045   {
00046     init(update_configs_vector, update_state);
00047     tactiles_vector->clear();
00048     for(unsigned int i=0;i<this->nb_tactiles;i++)
00049     {
00050       PST3Data tmp_pst(init_tactiles_vector->at(i));
00051       tactiles_vector->push_back( tmp_pst );
00052     }
00053   }
00054 
00055   template <class StatusType, class CommandType>
00056   void ShadowPSTs<StatusType, CommandType>::init(std::vector<generic_updater::UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00057   {
00058     // Tactile sensor real time publisher
00059     tactile_publisher = boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::ShadowPST> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::ShadowPST>(this->nodehandle_, "tactile", 4));
00060 
00061     //initialize the vector of tactiles
00062     tactiles_vector = boost::shared_ptr< std::vector<PST3Data> >( new std::vector<PST3Data>(this->nb_tactiles) );
00063     this->all_tactile_data = boost::shared_ptr<std::vector<AllTactileData> >( new std::vector<AllTactileData>(this->nb_tactiles) );
00064   }
00065 
00066   template <class StatusType, class CommandType>
00067   void ShadowPSTs<StatusType, CommandType>::update(StatusType* status_data)
00068   {
00069     int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
00070     //TODO: use memcopy instead?
00071     for( unsigned int id_sensor = 0; id_sensor < this->nb_tactiles; ++id_sensor)
00072     {
00073       switch( static_cast<int32u>(status_data->tactile_data_type) )
00074       {
00075         //TACTILE DATA
00076       case TACTILE_SENSOR_TYPE_PST3_PRESSURE_TEMPERATURE:
00077         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00078         {
00079           tactiles_vector->at(id_sensor).pressure = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00080           tactiles_vector->at(id_sensor).temperature = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[1]) );
00081           tactiles_vector->at(id_sensor).debug_1 = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00082           tactiles_vector->at(id_sensor).debug_2 = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[3]) );
00083         }
00084         break;
00085 
00086       case TACTILE_SENSOR_TYPE_PST3_PRESSURE_RAW_ZERO_TRACKING:
00087         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00088         {
00089           tactiles_vector->at(id_sensor).pressure_raw = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00090           tactiles_vector->at(id_sensor).zero_tracking = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[1]) );
00091         }
00092         break;
00093 
00094       case TACTILE_SENSOR_TYPE_PST3_DAC_VALUE:
00095         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00096         {
00097           tactiles_vector->at(id_sensor).dac_value = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00098         }
00099         break;
00100 
00101         //COMMON DATA
00102       case TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ:
00103         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00104         {
00105           tactiles_vector->at(id_sensor).sample_frequency = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00106         }
00107         break;
00108 
00109       case TACTILE_SENSOR_TYPE_MANUFACTURER:
00110       {
00111         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00112         {
00113           tactiles_vector->at(id_sensor).manufacturer = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00114         }
00115       }
00116       break;
00117 
00118       case TACTILE_SENSOR_TYPE_SERIAL_NUMBER:
00119       {
00120         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00121         {
00122           tactiles_vector->at(id_sensor).serial_number = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00123         }
00124       }
00125       break;
00126 
00127       case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
00128         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00129         {
00130           tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
00131         }
00132         break;
00133 
00134       case TACTILE_SENSOR_TYPE_PCB_VERSION:
00135         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00136         {
00137           tactiles_vector->at(id_sensor).pcb_version =  sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00138         }
00139         break;
00140 
00141       default:
00142         break;
00143 
00144       } //end switch
00145     } //end for tactile
00146 
00147     if(this->sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
00148     {
00149       this->process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
00150       if(this->sensor_updater->initialization_configs_vector.size() == 0)
00151         this->sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
00152     }
00153   }
00154 
00155   template <class StatusType, class CommandType>
00156   void ShadowPSTs<StatusType, CommandType>::publish()
00157   {
00158     if(tactile_publisher->trylock())
00159     {
00160       //for the time being, we only have PSTs tactile sensors
00161       sr_robot_msgs::ShadowPST tactiles;
00162       tactiles.header.stamp = ros::Time::now();
00163 
00164       //tactiles.pressure.push_back(sr_hand_lib->tactile_data_valid);
00165 
00166       for(unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00167       {
00168         //Always publish the last valid data: the data are updated
00169         // only if they are valid
00170         tactiles.pressure.push_back( tactiles_vector->at(id_tact).pressure );
00171         tactiles.temperature.push_back( tactiles_vector->at(id_tact).temperature );
00172       }
00173 
00174 
00175       tactile_publisher->msg_ = tactiles;
00176       tactile_publisher->unlockAndPublish();
00177     }
00178 
00179   }//end publish
00180 
00181   template <class StatusType, class CommandType>
00182   void ShadowPSTs<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00183                                    diagnostic_updater::DiagnosticStatusWrapper &d)
00184   {
00185     for(unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00186     {
00187       std::stringstream ss;
00188 
00189       ss << "Tactile " << id_tact + 1;
00190 
00191       d.name = ss.str().c_str();
00192       d.summary(d.OK, "OK");
00193       d.clear();
00194 
00195       d.addf("Sample Frequency", "%d", tactiles_vector->at(id_tact).sample_frequency);
00196       d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str());
00197       d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str());
00198 
00199       d.addf("Software Version", "%s", tactiles_vector->at(id_tact).get_software_version().c_str());
00200       d.addf("PCB Version", "%s", tactiles_vector->at(id_tact).pcb_version.c_str());
00201 
00202       d.addf("Pressure Raw", "%d", tactiles_vector->at(id_tact).pressure_raw);
00203       d.addf("Zero Tracking", "%d", tactiles_vector->at(id_tact).zero_tracking);
00204       d.addf("DAC Value", "%d", tactiles_vector->at(id_tact).dac_value);
00205 
00206       vec.push_back(d);
00207     }
00208   }
00209 
00210   template <class StatusType, class CommandType>
00211   std::vector<AllTactileData>* ShadowPSTs<StatusType, CommandType>::get_tactile_data()
00212   {
00213     for( unsigned int i=0; i < tactiles_vector->size(); ++i)
00214       this->all_tactile_data->at(i).pst = tactiles_vector->at(i);
00215 
00216     return this->all_tactile_data.get();
00217   }
00218 
00219   //Only to ensure that the template class is compiled for the types we are interested in
00220   template class ShadowPSTs<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00221   template class ShadowPSTs<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00222   template class ShadowPSTs<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00223 }
00224 
00225 /* For the emacs weenies in the crowd.
00226    Local Variables:
00227    c-basic-offset: 2
00228    End:
00229 */
00230 
00231