sr_robot_lib: biotac.cpp Source File

Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/biotac.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   Biotac<StatusType, CommandType>::Biotac(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   Biotac<StatusType, CommandType>::Biotac(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       BiotacData 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 Biotac<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::BiotacAll> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::BiotacAll>(this->nodehandle_, "tactile", 4));
00060 
00061     //initialize the vector of tactiles
00062     tactiles_vector = boost::shared_ptr< std::vector<BiotacData> >( new std::vector<BiotacData>(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 Biotac<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       //We always receive pac0 and pac1
00074       tactiles_vector->at(id_sensor).pac0 = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00075       tactiles_vector->at(id_sensor).pac1 = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[1]) );
00076 
00077       //the rest of the data is sampled at different rates
00078       switch( static_cast<int32u>(status_data->tactile_data_type) )
00079       {
00080         //TACTILE DATA
00081       case TACTILE_SENSOR_TYPE_BIOTAC_INVALID:
00082         ROS_WARN("received invalid tactile type");
00083         break;
00084 
00085       case TACTILE_SENSOR_TYPE_BIOTAC_PDC:
00086         tactiles_vector->at(id_sensor).pdc = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00087         break;
00088 
00089       case TACTILE_SENSOR_TYPE_BIOTAC_TAC:
00090         tactiles_vector->at(id_sensor).tac = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00091         break;
00092 
00093       case TACTILE_SENSOR_TYPE_BIOTAC_TDC:
00094         tactiles_vector->at(id_sensor).tdc = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00095         break;
00096 
00097       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1:
00098         tactiles_vector->at(id_sensor).electrodes[0] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00099         break;
00100 
00101       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2:
00102         tactiles_vector->at(id_sensor).electrodes[1] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00103         break;
00104 
00105       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3:
00106         tactiles_vector->at(id_sensor).electrodes[2] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00107         break;
00108 
00109       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4:
00110         tactiles_vector->at(id_sensor).electrodes[3] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00111         break;
00112 
00113       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5:
00114         tactiles_vector->at(id_sensor).electrodes[4] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00115         break;
00116 
00117       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6:
00118         tactiles_vector->at(id_sensor).electrodes[5] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00119         break;
00120 
00121       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7:
00122         tactiles_vector->at(id_sensor).electrodes[6] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00123         break;
00124 
00125       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8:
00126         tactiles_vector->at(id_sensor).electrodes[7] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00127         break;
00128 
00129       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9:
00130         tactiles_vector->at(id_sensor).electrodes[8] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00131         break;
00132 
00133       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10:
00134         tactiles_vector->at(id_sensor).electrodes[9] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00135         break;
00136 
00137       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11:
00138         tactiles_vector->at(id_sensor).electrodes[10] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00139         break;
00140 
00141       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12:
00142         tactiles_vector->at(id_sensor).electrodes[11] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00143         break;
00144 
00145       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13:
00146         tactiles_vector->at(id_sensor).electrodes[12] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00147         break;
00148 
00149       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14:
00150         tactiles_vector->at(id_sensor).electrodes[13] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00151         break;
00152 
00153       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15:
00154         tactiles_vector->at(id_sensor).electrodes[14] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00155         break;
00156 
00157       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16:
00158         tactiles_vector->at(id_sensor).electrodes[15] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00159         break;
00160 
00161       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17:
00162         tactiles_vector->at(id_sensor).electrodes[16] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00163         break;
00164 
00165       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18:
00166         tactiles_vector->at(id_sensor).electrodes[17] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00167         break;
00168 
00169       case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19:
00170         tactiles_vector->at(id_sensor).electrodes[18] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00171         break;
00172 
00173         //COMMON DATA
00174       case TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ:
00175         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00176         {
00177           tactiles_vector->at(id_sensor).sample_frequency = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00178         }
00179         break;
00180 
00181       case TACTILE_SENSOR_TYPE_MANUFACTURER:
00182         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00183         {
00184           tactiles_vector->at(id_sensor).manufacturer = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00185         }
00186         break;
00187 
00188       case TACTILE_SENSOR_TYPE_SERIAL_NUMBER:
00189         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00190         {
00191           tactiles_vector->at(id_sensor).serial_number = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00192         }
00193         break;
00194 
00195       case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
00196         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00197         {
00198           tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
00199         }
00200         break;
00201 
00202       case TACTILE_SENSOR_TYPE_PCB_VERSION:
00203         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00204         {
00205           tactiles_vector->at(id_sensor).pcb_version = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00206         }
00207         break;
00208 
00209       default:
00210         break;
00211 
00212       } //end switch
00213     } //end for tactile
00214 
00215     if(this->sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
00216     {
00217       this->process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
00218       if(this->sensor_updater->initialization_configs_vector.size() == 0)
00219         this->sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
00220     }
00221   }
00222 
00223   template <class StatusType, class CommandType>
00224   void Biotac<StatusType, CommandType>::publish()
00225   {
00226     if(tactile_publisher->trylock())
00227     {
00228       //for the time being, we only have PSTs tactile sensors
00229       sr_robot_msgs::BiotacAll tactiles;
00230       tactiles.header.stamp = ros::Time::now();
00231 
00232       for(unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00233       {
00234         sr_robot_msgs::Biotac tactile_tmp;
00235 
00236         tactile_tmp.pac0 = static_cast<int16u>(tactiles_vector->at(id_tact).pac0);
00237         tactile_tmp.pac1 = static_cast<int16u>(tactiles_vector->at(id_tact).pac1);
00238 
00239         tactile_tmp.pdc = static_cast<int16u>(tactiles_vector->at(id_tact).pdc);
00240         tactile_tmp.tac = static_cast<int16u>(tactiles_vector->at(id_tact).tac);
00241         tactile_tmp.tdc = static_cast<int16u>(tactiles_vector->at(id_tact).tdc);
00242 
00243         tactile_tmp.electrodes = tactiles_vector->at(id_tact).electrodes;
00244 
00245         tactiles.tactiles[id_tact] = tactile_tmp;
00246       }
00247 
00248       tactile_publisher->msg_ = tactiles;
00249       tactile_publisher->unlockAndPublish();
00250     }
00251 
00252   }//end publish
00253 
00254   template <class StatusType, class CommandType>
00255   void Biotac<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00256                                diagnostic_updater::DiagnosticStatusWrapper &d)
00257   {
00258     for(unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00259     {
00260       std::stringstream ss;
00261 
00262       ss << "Tactile " << id_tact + 1;
00263 
00264       d.name = ss.str().c_str();
00265       d.summary(d.OK, "OK");
00266       d.clear();
00267 
00268       d.addf("Sample Frequency", "%d", tactiles_vector->at(id_tact).sample_frequency);
00269       d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str());
00270       d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str());
00271 
00272       d.addf("Software Version", "%s", tactiles_vector->at(id_tact).get_software_version().c_str());
00273       d.addf("PCB Version", "%s", tactiles_vector->at(id_tact).pcb_version.c_str());
00274 
00275       vec.push_back(d);
00276     }
00277   }
00278 
00279   template <class StatusType, class CommandType>
00280   std::vector<AllTactileData>* Biotac<StatusType, CommandType>::get_tactile_data()
00281   {
00282     for( unsigned int i=0; i < tactiles_vector->size(); ++i)
00283       this->all_tactile_data->at(i).biotac = tactiles_vector->at(i);
00284 
00285     return this->all_tactile_data.get();
00286   }
00287 
00288   //Only to ensure that the template class is compiled for the types we are interested in
00289   template class Biotac<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00290   template class Biotac<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00291   template class Biotac<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00292 }
00293 
00294 /* For the emacs weenies in the crowd.
00295    Local Variables:
00296    c-basic-offset: 2
00297    End:
00298 */
00299 
00300