generic_tactiles.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/generic_tactiles.hpp"
00029 #include <sr_utilities/sr_math_utils.hpp>
00030 #include <cctype>
00031 
00032 namespace tactiles
00033 {
00034   const unsigned int GenericTactiles::nb_tactiles = 5;
00035 
00036   GenericTactiles::GenericTactiles(std::vector<generic_updater::UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00037   {
00038     sensor_updater = boost::shared_ptr<generic_updater::SensorUpdater>(new generic_updater::SensorUpdater(update_configs_vector, update_state));
00039     if(update_state != operation_mode::device_update_state::INITIALIZATION)
00040     {
00041       reset_service_client_ = nodehandle_.advertiseService("/tactiles/reset", &GenericTactiles::reset, this);
00042     }
00043 
00044     //initialize the vector of tactiles
00045     tactiles_vector = boost::shared_ptr< std::vector<GenericTactileData> >( new std::vector<GenericTactileData>(nb_tactiles) );
00046     all_tactile_data = boost::shared_ptr<std::vector<AllTactileData> >( new std::vector<AllTactileData>(nb_tactiles) );
00047 
00048     for(unsigned int i=0;i<nb_tactiles;i++)
00049     {
00050       GenericTactileData tmp_pst;
00051       tactiles_vector->push_back( tmp_pst );
00052     }
00053   }
00054 
00055 
00056   void GenericTactiles::update(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_STATUS* status_data)
00057   {
00058     int tactile_mask = static_cast<int16u>(status_data->tactile_data_valid);
00059     //TODO: use memcopy instead?
00060     for( unsigned int id_sensor = 0; id_sensor < nb_tactiles; ++id_sensor)
00061     {
00062       ROS_DEBUG_STREAM(" received: " << static_cast<int32u>(status_data->tactile_data_type));
00063 
00064       switch( static_cast<int32u>(status_data->tactile_data_type) )
00065       {
00066       //COMMON DATA
00067       case TACTILE_SENSOR_TYPE_WHICH_SENSORS:
00068         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00069         {
00070           if( tactiles_vector != NULL )
00071             tactiles_vector->at(id_sensor).which_sensor = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00072           ROS_DEBUG_STREAM(" tact["<<id_sensor<<"] = " << tactiles_vector->at(id_sensor).which_sensor);
00073 
00074         }
00075         break;
00076 
00077       case TACTILE_SENSOR_TYPE_SAMPLE_FREQUENCY_HZ:
00078         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00079         {
00080           if( tactiles_vector != NULL )
00081             tactiles_vector->at(id_sensor).sample_frequency = static_cast<unsigned int>(static_cast<int16u>(status_data->tactile[id_sensor].word[0]) );
00082         }
00083         break;
00084 
00085       case TACTILE_SENSOR_TYPE_MANUFACTURER:
00086       {
00087         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00088         {
00089           tactiles_vector->at(id_sensor).manufacturer = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00090         }
00091       }
00092       break;
00093 
00094       case TACTILE_SENSOR_TYPE_SERIAL_NUMBER:
00095       {
00096         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00097         {
00098           tactiles_vector->at(id_sensor).serial_number = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00099         }
00100       }
00101       break;
00102 
00103       case TACTILE_SENSOR_TYPE_SOFTWARE_VERSION:
00104         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00105         {
00106           if( tactiles_vector != NULL )
00107             tactiles_vector->at(id_sensor).set_software_version( status_data->tactile[id_sensor].string );
00108         }
00109         break;
00110 
00111       case TACTILE_SENSOR_TYPE_PCB_VERSION:
00112         if( sr_math_utils::is_bit_mask_index_true(tactile_mask, id_sensor) )
00113         {
00114           if( tactiles_vector != NULL )
00115             tactiles_vector->at(id_sensor).pcb_version = sanitise_string( status_data->tactile[id_sensor].string, TACTILE_DATA_LENGTH_BYTES );
00116         }
00117         break;
00118 
00119       default:
00120         break;
00121 
00122       } //end switch
00123     } //end for tactile
00124 
00125     if(sensor_updater->update_state == operation_mode::device_update_state::INITIALIZATION)
00126     {
00127       process_received_data_type(static_cast<int32u>(status_data->tactile_data_type));
00128       if(sensor_updater->initialization_configs_vector.size() == 0)
00129         sensor_updater->update_state = operation_mode::device_update_state::OPERATION;
00130     }
00131   }
00132 
00133   void GenericTactiles::process_received_data_type(int32u data)
00134   {
00135     unsigned int i;
00136     for(i=0; i<sensor_updater->initialization_configs_vector.size(); i++)
00137     {
00138       if (sensor_updater->initialization_configs_vector[i].what_to_update == data) break;
00139     }
00140     if(i<sensor_updater->initialization_configs_vector.size())
00141       sensor_updater->initialization_configs_vector.erase(sensor_updater->initialization_configs_vector.begin() + i);
00142   }
00143 
00144   void GenericTactiles::publish()
00145   {
00146     //We don't publish anything during the initialization phase
00147   }//end publish
00148 
00149   void GenericTactiles::add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00150                                         diagnostic_updater::DiagnosticStatusWrapper &d)
00151   {
00152     //We don't publish diagnostics during the initialization phase
00153   }
00154 
00163   bool GenericTactiles::reset(std_srvs::Empty::Request& request,
00164                               std_srvs::Empty::Response& response)
00165   {
00166     ROS_INFO_STREAM("Resetting tactiles");
00167 
00168     return sensor_updater->reset();
00169   }
00170 
00171 
00172   std::string GenericTactiles::sanitise_string( const char* raw_string, const unsigned int str_size )
00173   {
00174     std::string sanitised_string = "";
00175     for (unsigned int i = 0; i < str_size; ++i)
00176     {
00177       char tmp = static_cast<char>( raw_string[i] );
00178       if( tmp != 0 )
00179       {
00180         if( tmp >= '\x20' && tmp <= '\x7E')
00181         {
00182           sanitised_string += static_cast<char>( raw_string[i] ) ;
00183         }
00184         else
00185           sanitised_string += '?';
00186       }
00187       else
00188         break;
00189     }
00190     return sanitised_string;
00191   }
00192 
00193   std::vector<AllTactileData>* GenericTactiles::get_tactile_data()
00194   {
00195     return all_tactile_data.get();
00196   }
00197 
00198 }
00199 
00200 /* For the emacs weenies in the crowd.
00201    Local Variables:
00202    c-basic-offset: 2
00203    End:
00204 */
00205 
00206 


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Thu Jan 2 2014 12:02:16