UBI0.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/UBI0.hpp"
00029 #include <sr_utilities/sr_math_utils.hpp>
00030 
00031 #define TACTILE_DATA_LENGTH_BYTES TACTILE_DATA_LENGTH_BYTES_v2
00032 
00033 namespace tactiles
00034 {
00035   template <class StatusType, class CommandType>
00036   UBI0<StatusType, CommandType>::UBI0(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   UBI0<StatusType, CommandType>::UBI0(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       UBI0Data 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 UBI0<StatusType, CommandType>::init(std::vector<generic_updater::UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00057   {
00058     // Distal tactile sensor real time publisher
00059     tactile_publisher = boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::UBI0All> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::UBI0All>(this->nodehandle_ , "tactile", 4));
00060     // Middle and proximal tactile sensor real time publisher
00061     mid_prox_publisher = boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::MidProxDataAll> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::MidProxDataAll>(this->nodehandle_ , "tactile_mid_prox", 4));
00062     // Auxiliar Spi data (sometimes it is a palm tactile sensor) real time publisher
00063     aux_spi_publisher = boost::shared_ptr<realtime_tools::RealtimePublisher<sr_robot_msgs::AuxSpiData> >( new realtime_tools::RealtimePublisher<sr_robot_msgs::AuxSpiData>(this->nodehandle_ , "tactile_aux_spi", 4));
00064 
00065     //initialize the vector of tactiles
00066     tactiles_vector = boost::shared_ptr< std::vector<UBI0Data> >( new std::vector<UBI0Data>(this->nb_tactiles) );
00067     this->all_tactile_data = boost::shared_ptr<std::vector<AllTactileData> >( new std::vector<AllTactileData>(this->nb_tactiles) );
00068 
00069     //initialize the palm sensors
00070     palm_tactiles = boost::shared_ptr< UBI0PalmData >( new UBI0PalmData() );
00071   }
00072 
00073   template <class StatusType, class CommandType>
00074   void UBI0<StatusType, CommandType>::update(StatusType* status_data)
00075   {
00076     int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
00077     //TODO: use memcopy instead?
00078     for( unsigned int id_sensor = 0; id_sensor < this->nb_tactiles; ++id_sensor)
00079     {
00080       //the rest of the data is sampled at different rates
00081       switch( static_cast<int32u>(status_data->tactile_data_type) )
00082       {
00083         //TACTILE DATA
00084       case TACTILE_SENSOR_TYPE_UBI0_TACTILE:
00085         for( unsigned int i = 0; i < tactiles_vector->at(id_sensor).distal.size(); ++i)
00086         {
00087           tactiles_vector->at(id_sensor).distal[i] = static_cast<int>(static_cast<int16u>(status_data->tactile[id_sensor].word[i]) );
00088         }
00089         for( unsigned int i = 0; i < tactiles_vector->at(id_sensor).middle.size(); ++i)
00090         {
00091           tactiles_vector->at(id_sensor).middle[i] = static_cast<int>(static_cast<int16u>(status_data->tactile_mid_prox[id_sensor].named.middle[i]) );
00092         }
00093         for( unsigned int i = 0; i < tactiles_vector->at(id_sensor).proximal.size(); ++i)
00094         {
00095           tactiles_vector->at(id_sensor).proximal[i] = static_cast<int>(static_cast<int16u>(status_data->tactile_mid_prox[id_sensor].named.proximal[i]) );
00096         }
00097         break;
00098 
00099         //COMMON DATA
00100       case TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ:
00101         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00102         {
00103           tactiles_vector->at(id_sensor).sample_frequency = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00104         }
00105         break;
00106 
00107       case TACTILE_SENSOR_TYPE_MANUFACTURER:
00108       {
00109         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00110         {
00111           tactiles_vector->at(id_sensor).manufacturer = this->sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00112         }
00113       }
00114       break;
00115 
00116       case TACTILE_SENSOR_TYPE_SERIAL_NUMBER:
00117       {
00118         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00119         {
00120           tactiles_vector->at(id_sensor).serial_number = this->sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00121         }
00122       }
00123       break;
00124 
00125       case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
00126         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00127         {
00128           tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
00129         }
00130         break;
00131 
00132       case TACTILE_SENSOR_TYPE_PCB_VERSION:
00133         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00134         {
00135           tactiles_vector->at(id_sensor).pcb_version = this->sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00136         }
00137         break;
00138 
00139       default:
00140         break;
00141 
00142       } //end switch
00143     } //end for tactile
00144 
00145     for( unsigned int i = 0; i < palm_tactiles->palm.size(); ++i)
00146     {
00147       palm_tactiles->palm[i] = static_cast<int>(static_cast<int16u>(status_data->aux_spi_sensor.sensor[i]) );
00148     }
00149 
00150     if(this->sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
00151     {
00152       this->process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
00153       if(this->sensor_updater->initialization_configs_vector.size() == 0)
00154         this->sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
00155     }
00156   }
00157 
00158   template <class StatusType, class CommandType>
00159   void UBI0<StatusType, CommandType>::publish()
00160   {
00161     if(tactile_publisher->trylock())
00162     {
00163       sr_robot_msgs::UBI0All tactiles;
00164       tactiles.header.stamp = ros::Time::now();
00165 
00166       //tactiles.pressure.push_back(sr_hand_lib->tactile_data_valid);
00167 
00168       for(unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00169       {
00170         sr_robot_msgs::UBI0 tactile_tmp;
00171 
00172         tactile_tmp.distal = tactiles_vector->at(id_tact).distal;
00173 
00174         tactiles.tactiles[id_tact] = tactile_tmp;
00175       }
00176 
00177       tactile_publisher->msg_ = tactiles;
00178       tactile_publisher->unlockAndPublish();
00179     }
00180 
00181     if(mid_prox_publisher->trylock())
00182     {
00183       sr_robot_msgs::MidProxDataAll tactiles;
00184       tactiles.header.stamp = ros::Time::now();
00185 
00186       for(unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00187       {
00188         sr_robot_msgs::MidProxData tactile_tmp;
00189 
00190         tactile_tmp.middle = tactiles_vector->at(id_tact).middle;
00191         tactile_tmp.proximal = tactiles_vector->at(id_tact).proximal;
00192 
00193         tactiles.sensors[id_tact] = tactile_tmp;
00194       }
00195 
00196       mid_prox_publisher->msg_ = tactiles;
00197       mid_prox_publisher->unlockAndPublish();
00198     }
00199 
00200     if(aux_spi_publisher->trylock())
00201     {
00202       sr_robot_msgs::AuxSpiData tactiles;
00203       tactiles.header.stamp = ros::Time::now();
00204 
00205       tactiles.sensors = palm_tactiles->palm;
00206 
00207       aux_spi_publisher->msg_ = tactiles;
00208       aux_spi_publisher->unlockAndPublish();
00209     }
00210 
00211   }//end publish
00212 
00213   template <class StatusType, class CommandType>
00214   void UBI0<StatusType, CommandType>::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00215                                diagnostic_updater::DiagnosticStatusWrapper &d)
00216   {
00217     for(unsigned int id_tact = 0; id_tact < this->nb_tactiles; ++id_tact)
00218     {
00219       std::stringstream ss;
00220 
00221       ss << "Tactile " << id_tact + 1;
00222 
00223       d.name = ss.str().c_str();
00224       d.summary(d.OK, "OK");
00225       d.clear();
00226 
00227       d.addf("Sample Frequency", "%d", tactiles_vector->at(id_tact).sample_frequency);
00228       d.addf("Manufacturer", "%s", tactiles_vector->at(id_tact).manufacturer.c_str());
00229       d.addf("Serial Number", "%s", tactiles_vector->at(id_tact).serial_number.c_str());
00230 
00231       d.addf("Software Version", "%s", tactiles_vector->at(id_tact).get_software_version().c_str());
00232       d.addf("PCB Version", "%s", tactiles_vector->at(id_tact).pcb_version.c_str());
00233 
00234       vec.push_back(d);
00235     }
00236   }
00237 
00238   template <class StatusType, class CommandType>
00239   std::vector<AllTactileData>* UBI0<StatusType, CommandType>::get_tactile_data()
00240   {
00241     for( unsigned int i=0; i < tactiles_vector->size(); ++i)
00242       this->all_tactile_data->at(i).ubi0 = tactiles_vector->at(i);
00243 
00244     return this->all_tactile_data.get();
00245   }
00246 
00247   //Only to ensure that the template class is compiled for the types we are interested in
00248   template class UBI0<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00249   template class UBI0<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00250   template class UBI0<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00251 }
00252 
00253 /* For the emacs weenies in the crowd.
00254    Local Variables:
00255    c-basic-offset: 2
00256    End:
00257 */
00258 


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Thu Aug 27 2015 15:16:37