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