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 #include <string>
00031 #include <vector>
00032 
00033 #define TACTILE_DATA_LENGTH_BYTES TACTILE_DATA_LENGTH_BYTES_v1
00034 
00035 namespace tactiles
00036 {
00037 
00038   template <class StatusType, class CommandType>
00039   const size_t Biotac<StatusType, CommandType>::nb_electrodes_v1_ = 19;
00040 
00041   template <class StatusType, class CommandType>
00042   const size_t Biotac<StatusType, CommandType>::nb_electrodes_v2_ = 24;
00043 
00044   template<class StatusType, class CommandType>
00045   Biotac<StatusType, CommandType>::Biotac(ros::NodeHandle nh, std::string device_id,
00046                                           std::vector<generic_updater::UpdateConfig> update_configs_vector,
00047                                           operation_mode::device_update_state::DeviceUpdateState update_state)
00048           : GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
00049   {
00050     init(update_configs_vector, update_state);
00051   }
00052 
00053   template<class StatusType, class CommandType>
00054   Biotac<StatusType, CommandType>::Biotac(ros::NodeHandle nh, std::string device_id,
00055                                           std::vector<generic_updater::UpdateConfig> update_configs_vector,
00056                                           operation_mode::device_update_state::DeviceUpdateState update_state,
00057                                           boost::shared_ptr<std::vector<GenericTactileData> > init_tactiles_vector)
00058           : GenericTactiles<StatusType, CommandType>(nh, device_id, update_configs_vector, update_state)
00059   {
00060     init(update_configs_vector, update_state);
00061     tactiles_vector->clear();
00062     for (unsigned int i = 0; i < this->nb_tactiles; i++)
00063     {
00064       BiotacData tmp_pst(init_tactiles_vector->at(i));
00065       tactiles_vector->push_back(tmp_pst);
00066     }
00067 
00068     set_version_specific_details();
00069   }
00070 
00071   template<class StatusType, class CommandType>
00072   void Biotac<StatusType, CommandType>::init(std::vector<generic_updater::UpdateConfig> update_configs_vector,
00073                                              operation_mode::device_update_state::DeviceUpdateState update_state)
00074   {
00075     // initialize the vector of tactiles
00076     tactiles_vector = boost::shared_ptr<std::vector<BiotacData> >(new std::vector<BiotacData>(this->nb_tactiles));
00077     this->all_tactile_data = boost::shared_ptr<std::vector<AllTactileData> >(
00078             new std::vector<AllTactileData>(this->nb_tactiles));
00079 
00080     for (size_t i = 0; i < this->all_tactile_data->size(); ++i)
00081     {
00082       this->all_tactile_data->at(i).type = "biotac";
00083     }
00084   }
00085 
00086   template<class StatusType, class CommandType>
00087   void Biotac<StatusType, CommandType>::update(StatusType *status_data)
00088   {
00089     int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
00090     // @todo use memcopy instead?
00091     for (unsigned int id_sensor = 0; id_sensor < this->nb_tactiles; ++id_sensor)
00092     {
00093       TACTILE_SENSOR_BIOTAC_DATA_CONTENTS *tactile_data =
00094         reinterpret_cast<TACTILE_SENSOR_BIOTAC_DATA_CONTENTS*> (&(status_data->tactile[id_sensor]));
00095       // We always receive pac0 and pac1
00096       tactiles_vector->at(id_sensor).pac0 = static_cast<int>(tactile_data->Pac[0]);
00097       tactiles_vector->at(id_sensor).pac1 = static_cast<int>(tactile_data->Pac[1]);
00098 
00099       // the rest of the data is sampled at different rates
00100       switch (static_cast<int32u>(status_data->tactile_data_type))
00101       {
00102         // TACTILE DATA
00103         case TACTILE_SENSOR_TYPE_BIOTAC_INVALID:
00104           ROS_WARN("received invalid tactile type");
00105           break;
00106 
00107         case TACTILE_SENSOR_TYPE_BIOTAC_PDC:
00108           if (tactile_data->data_valid.other_sensor_0)
00109           {
00110             tactiles_vector->at(id_sensor).pdc = static_cast<int>(tactile_data->other_sensor_0);
00111           }
00112           else
00113           {
00114             // TODO(shadow_team): add some error stats
00115           }
00116           if (tactile_data->data_valid.other_sensor_1)
00117           {
00118             tactiles_vector->at(id_sensor).tac = static_cast<int>(tactile_data->other_sensor_1);
00119           }
00120           break;
00121 
00122         // case TACTILE_SENSOR_TYPE_BIOTAC_TAC:
00123         //   tactiles_vector->at(id_sensor).tac =
00124         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]));
00125         //   break;
00126 
00127         case TACTILE_SENSOR_TYPE_BIOTAC_TDC:
00128           if (tactile_data->data_valid.other_sensor_0)
00129           {
00130             tactiles_vector->at(id_sensor).tdc = static_cast<int>(tactile_data->other_sensor_0);
00131           }
00132           if (tactile_data->data_valid.other_sensor_1)
00133           {
00134             tactiles_vector->at(id_sensor).electrodes[0] = static_cast<int>(tactile_data->other_sensor_1);
00135           }
00136           break;
00137 
00138         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_1:
00139         //   tactiles_vector->at(id_sensor).electrodes[0] =
00140         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00141         //   break;
00142 
00143         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_2:
00144           if (tactile_data->data_valid.other_sensor_0)
00145           {
00146             tactiles_vector->at(id_sensor).electrodes[1] = static_cast<int>(tactile_data->other_sensor_0);
00147           }
00148           if (tactile_data->data_valid.other_sensor_1)
00149           {
00150             tactiles_vector->at(id_sensor).electrodes[2] = static_cast<int>(tactile_data->other_sensor_1);
00151           }
00152           break;
00153 
00154         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_3:
00155         //   tactiles_vector->at(id_sensor).electrodes[2] =
00156         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00157         //   break;
00158 
00159         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_4:
00160           if (tactile_data->data_valid.other_sensor_0)
00161           {
00162             tactiles_vector->at(id_sensor).electrodes[3] = static_cast<int>(tactile_data->other_sensor_0);
00163           }
00164           if (tactile_data->data_valid.other_sensor_1)
00165           {
00166             tactiles_vector->at(id_sensor).electrodes[4] = static_cast<int>(tactile_data->other_sensor_1);
00167           }
00168           break;
00169 
00170         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_5:
00171         //   tactiles_vector->at(id_sensor).electrodes[4] =
00172         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00173         //   break;
00174 
00175         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_6:
00176           if (tactile_data->data_valid.other_sensor_0)
00177           {
00178             tactiles_vector->at(id_sensor).electrodes[5] = static_cast<int>(tactile_data->other_sensor_0);
00179           }
00180           if (tactile_data->data_valid.other_sensor_1)
00181           {
00182             tactiles_vector->at(id_sensor).electrodes[6] = static_cast<int>(tactile_data->other_sensor_1);
00183           }
00184           break;
00185 
00186         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_7:
00187         //   tactiles_vector->at(id_sensor).electrodes[6] =
00188         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00189         //   break;
00190 
00191         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_8:
00192           if (tactile_data->data_valid.other_sensor_0)
00193           {
00194             tactiles_vector->at(id_sensor).electrodes[7] = static_cast<int>(tactile_data->other_sensor_0);
00195           }
00196           if (tactile_data->data_valid.other_sensor_1)
00197           {
00198             tactiles_vector->at(id_sensor).electrodes[8] = static_cast<int>(tactile_data->other_sensor_1);
00199           }
00200           break;
00201 
00202         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_9:
00203         //  tactiles_vector->at(id_sensor).electrodes[8] =
00204         //    static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00205         //  break;
00206 
00207         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_10:
00208           if (tactile_data->data_valid.other_sensor_0)
00209           {
00210             tactiles_vector->at(id_sensor).electrodes[9] = static_cast<int>(tactile_data->other_sensor_0);
00211           }
00212           if (tactile_data->data_valid.other_sensor_1)
00213           {
00214             tactiles_vector->at(id_sensor).electrodes[10] = static_cast<int>(tactile_data->other_sensor_1);
00215           }
00216           break;
00217 
00218         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_11:
00219         //    tactiles_vector->at(id_sensor).electrodes[10] =
00220         //      static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00221         //    break;
00222 
00223         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_12:
00224           if (tactile_data->data_valid.other_sensor_0)
00225           {
00226             tactiles_vector->at(id_sensor).electrodes[11] = static_cast<int>(tactile_data->other_sensor_0);
00227           }
00228           if (tactile_data->data_valid.other_sensor_1)
00229           {
00230             tactiles_vector->at(id_sensor).electrodes[12] = static_cast<int>(tactile_data->other_sensor_1);
00231           }
00232           break;
00233 
00234         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_13:
00235         //   tactiles_vector->at(id_sensor).electrodes[12] =
00236         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00237         //   break;
00238 
00239         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_14:
00240           if (tactile_data->data_valid.other_sensor_0)
00241           {
00242             tactiles_vector->at(id_sensor).electrodes[13] = static_cast<int>(tactile_data->other_sensor_0);
00243           }
00244           if (tactile_data->data_valid.other_sensor_1)
00245           {
00246             tactiles_vector->at(id_sensor).electrodes[14] = static_cast<int>(tactile_data->other_sensor_1);
00247           }
00248           break;
00249 
00250         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_15:
00251         //   tactiles_vector->at(id_sensor).electrodes[14] =
00252         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00253         //   break;
00254 
00255         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_16:
00256           if (tactile_data->data_valid.other_sensor_0)
00257           {
00258             tactiles_vector->at(id_sensor).electrodes[15] = static_cast<int>(tactile_data->other_sensor_0);
00259           }
00260           if (tactile_data->data_valid.other_sensor_1)
00261           {
00262             tactiles_vector->at(id_sensor).electrodes[16] = static_cast<int>(tactile_data->other_sensor_1);
00263           }
00264           break;
00265 
00266         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_17:
00267         //   tactiles_vector->at(id_sensor).electrodes[16] =
00268         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00269         //   break;
00270 
00271         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_18:
00272           if (tactile_data->data_valid.other_sensor_0)
00273           {
00274             tactiles_vector->at(id_sensor).electrodes[17] = static_cast<int>(tactile_data->other_sensor_0);
00275           }
00276           if (tactile_data->data_valid.other_sensor_1)
00277           {
00278             tactiles_vector->at(id_sensor).electrodes[18] = static_cast<int>(tactile_data->other_sensor_1);
00279           }
00280           break;
00281 
00282         // case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_19:
00283         //   tactiles_vector->at(id_sensor).electrodes[18] =
00284         //     static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[2]) );
00285         //   break;
00286 
00287         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_20:
00288           if (tactile_data->data_valid.other_sensor_0)
00289           {
00290             tactiles_vector->at(id_sensor).electrodes[19] = static_cast<int>(tactile_data->other_sensor_0);
00291           }
00292           if (tactile_data->data_valid.other_sensor_1)
00293           {
00294             tactiles_vector->at(id_sensor).electrodes[20] = static_cast<int>(tactile_data->other_sensor_1);
00295           }
00296           break;
00297 
00298         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_22:
00299           if (tactile_data->data_valid.other_sensor_0)
00300           {
00301             tactiles_vector->at(id_sensor).electrodes[21] = static_cast<int>(tactile_data->other_sensor_0);
00302           }
00303           if (tactile_data->data_valid.other_sensor_1)
00304           {
00305             tactiles_vector->at(id_sensor).electrodes[22] = static_cast<int>(tactile_data->other_sensor_1);
00306           }
00307           break;
00308 
00309         case TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_24:
00310           if (tactile_data->data_valid.other_sensor_0)
00311           {
00312             tactiles_vector->at(id_sensor).electrodes[23] = static_cast<int>(tactile_data->other_sensor_0);
00313           }
00314           // if (tactile_data->data_valid.other_sensor_1)
00315           // {
00316           //   tactiles_vector->at(id_sensor).pdc = static_cast<int>(tactile_data->other_sensor_1);
00317           // }
00318           break;
00319 
00320           // COMMON DATA
00321         case TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ:
00322           if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
00323           {
00324             tactiles_vector->at(id_sensor).sample_frequency =
00325                     static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]));
00326           }
00327           break;
00328 
00329         case TACTILE_SENSOR_TYPE_MANUFACTURER:
00330           if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
00331           {
00332             tactiles_vector->at(id_sensor).manufacturer = this->sanitise_string(status_data->tactile[id_sensor].string,
00333                                                                                 TACTILE_DATA_LENGTH_BYTES);
00334           }
00335           break;
00336 
00337         case TACTILE_SENSOR_TYPE_SERIAL_NUMBER:
00338           if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
00339           {
00340             tactiles_vector->at(id_sensor).serial_number = this->sanitise_string(status_data->tactile[id_sensor].string,
00341                                                                                  TACTILE_DATA_LENGTH_BYTES);
00342           }
00343           break;
00344 
00345         case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
00346           if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
00347           {
00348             tactiles_vector->at(id_sensor).set_software_version(status_data->tactile[id_sensor].string);
00349           }
00350           break;
00351 
00352         case TACTILE_SENSOR_TYPE_PCB_VERSION:
00353           if (sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor))
00354           {
00355             tactiles_vector->at(id_sensor).pcb_version = this->sanitise_string(status_data->tactile[id_sensor].string,
00356                                                                                TACTILE_DATA_LENGTH_BYTES);
00357           }
00358           break;
00359 
00360         default:
00361           break;
00362       }  // end switch
00363     }  // end for tactile
00364 
00365     if (this->sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
00366     {
00367       this->process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
00368       if (this->sensor_updater->initialization_configs_vector.size() == 0)
00369       {
00370         this->sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
00371       }
00372     }
00373   }
00374 
00375   template<class StatusType, class CommandType>
00376   void Biotac<StatusType, CommandType>::publish()
00377   {
00378     // left empty, this is published from the controller publisher
00379   }  // end publish
00380 
00381   template <class StatusType, class CommandType>
00382   void Biotac<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00383                                                         diagnostic_updater::DiagnosticStatusWrapper &d)
00384   {
00385     for (unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00386     {
00387       std::stringstream ss;
00388       std::string prefix = this->device_id_.empty() ? this->device_id_ : (this->device_id_ + " ");
00389 
00390       ss << prefix << "Tactile " << id_tact + 1;
00391 
00392       d.name = ss.str().c_str();
00393       d.summary(d.OK, "OK");
00394       d.clear();
00395 
00396       d.addf("Sample Frequency", "%d", tactiles_vector->at(id_tact).sample_frequency);
00397       d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str());
00398       d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str());
00399 
00400       d.addf("Software Version", "%s", tactiles_vector->at(id_tact).get_software_version().c_str());
00401       d.addf("PCB Version", "%s", tactiles_vector->at(id_tact).pcb_version.c_str());
00402 
00403       vec.push_back(d);
00404     }
00405   }
00406 
00407   template<class StatusType, class CommandType>
00408   std::vector<AllTactileData> *Biotac<StatusType, CommandType>::get_tactile_data()
00409   {
00410     for (unsigned int i = 0; i < tactiles_vector->size(); ++i)
00411     {
00412       this->all_tactile_data->at(i).biotac = tactiles_vector->at(i);
00413     }
00414 
00415     return this->all_tactile_data.get();
00416   }
00417 
00418   template <class StatusType, class CommandType>
00419   void Biotac<StatusType, CommandType>::set_version_specific_details()
00420   {
00421     nb_electrodes_ = nb_electrodes_v1_;  // We consider biotac version one to be the default
00422 
00423     for (size_t i = 0; i < this->nb_tactiles; ++i)
00424     {
00425       // At least one of the fingers has a biotac version 2
00426       if (tactiles_vector->at(i).serial_number.find("BTSP") != std::string::npos)
00427       {
00428         nb_electrodes_ = nb_electrodes_v2_;
00429         break;
00430       }
00431     }
00432 
00433     if (nb_electrodes_ == nb_electrodes_v1_)  // If biotac version 1 remove polling for non-existing electrodes
00434     {
00435       for (int32u data = TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_20;
00436            data <= TACTILE_SENSOR_TYPE_BIOTAC_ELECTRODE_24; ++data)
00437       {
00438         std::vector<generic_updater::UpdateConfig>::iterator it =
00439           this->sensor_updater->important_update_configs_vector.begin();
00440         while (it != this->sensor_updater->important_update_configs_vector.end())
00441         {
00442           if (it->what_to_update == data)
00443           {
00444             it = this->sensor_updater->important_update_configs_vector.erase(it);
00445           }
00446           else
00447           {
00448             it++;
00449           }
00450         }
00451       }
00452     }
00453 
00454     for (unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00455     {
00456       tactiles_vector->at(id_tact).electrodes.resize(nb_electrodes_);
00457     }
00458   }
00459 
00460   // Only to ensure that the template class is compiled for the types we are interested in
00461   template
00462   class Biotac<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00463 
00464   template
00465   class Biotac<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00466 
00467   template
00468   class Biotac<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00469 }  // namespace tactiles
00470 
00471 /* For the emacs weenies in the crowd.
00472    Local Variables:
00473    c-basic-offset: 2
00474    End:
00475 */