sensor_updater.cpp
Go to the documentation of this file.
00001 
00028 #include "sr_robot_lib/sensor_updater.hpp"
00029 #include <boost/foreach.hpp>
00030 #include <iostream>
00031 
00032 namespace generic_updater
00033 {
00034   template <class CommandType>
00035   SensorUpdater<CommandType>::SensorUpdater(std::vector<UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00036     : GenericUpdater<CommandType>(update_configs_vector, update_state)
00037   {
00038   }
00039 
00040   template <class CommandType>
00041   SensorUpdater<CommandType>::~SensorUpdater()
00042   {
00043   }
00044 
00045   template <class CommandType>
00046   operation_mode::device_update_state::DeviceUpdateState SensorUpdater<CommandType>::build_init_command(CommandType* command)
00047   {
00048     if(!this->mutex->try_lock())
00049       return this->update_state;
00050 
00051     if (this->update_state == operation_mode::device_update_state::INITIALIZATION)
00052     {
00053       if(this->initialization_configs_vector.size()>0)
00054       {
00056         // First we ask for the next data we want to receive
00057 
00058         this->which_data_to_request ++;
00059 
00060         if( this->which_data_to_request >= this->initialization_configs_vector.size() )
00061           this->which_data_to_request = 0;
00062 
00063         //initialization data
00064         command->tactile_data_type = this->initialization_configs_vector[this->which_data_to_request].what_to_update;
00065         ROS_DEBUG_STREAM("Updating sensor initialization data type: "<<command->tactile_data_type << " | ["<<this->which_data_to_request<<"/"<<this->initialization_configs_vector.size()<<"] ");
00066       }
00067     }
00068     else
00069     {
00070       //For the last message sent when a change of update_state happens (after that we use build_command instead of build_init_command)
00071       //we use the TACTILE_SENSOR_TYPE_WHICH_SENSORS message, which is supposed to be always implemented
00072       //This is to avoid sending a random command (initialization_configs_vector is empty at this time)
00073       ROS_DEBUG_STREAM("Important data size: "<<this->important_update_configs_vector.size());
00074 
00075 
00076       command->tactile_data_type = TACTILE_SENSOR_TYPE_WHICH_SENSORS;
00077       ROS_DEBUG_STREAM("Updating sensor initialization data type: "<<command->tactile_data_type << " | ["<<this->which_data_to_request<<"/"<<this->important_update_configs_vector.size()<<"] ");
00078     }
00079     this->mutex->unlock();
00080 
00081     return this->update_state;
00082   }
00083 
00084   template <class CommandType>
00085   operation_mode::device_update_state::DeviceUpdateState SensorUpdater<CommandType>::build_command(CommandType* command)
00086   {
00087     if(!this->mutex->try_lock())
00088       return this->update_state;
00089 
00091     // First we ask for the next data we want to receive
00092 
00093     this->which_data_to_request ++;
00094 
00095     if( this->which_data_to_request >= this->important_update_configs_vector.size() )
00096       this->which_data_to_request = 0;
00097 
00098     if(!this->unimportant_data_queue.empty())
00099     {
00100       //an unimportant data is available
00101       command->tactile_data_type = this->unimportant_data_queue.front();
00102       this->unimportant_data_queue.pop();
00103 
00104       ROS_DEBUG_STREAM("Updating sensor unimportant data type: "<<command->tactile_data_type << " | queue size: "<<this->unimportant_data_queue.size());
00105     }
00106     else
00107     {
00108       //important data to update as often as possible
00109       command->tactile_data_type = this->important_update_configs_vector[this->which_data_to_request].what_to_update;
00110       ROS_DEBUG_STREAM("Updating sensor important data type: "<<command->tactile_data_type << " | ["<<this->which_data_to_request<<"/"<<this->important_update_configs_vector.size()<<"] ");
00111     }
00112 
00113     this->mutex->unlock();
00114 
00115     return this->update_state;
00116   }
00117 
00118   template <class CommandType>
00119   bool SensorUpdater<CommandType>::reset()
00120   {
00121     //We need to send the reset command twice in a row to make sure
00122     // the tactiles are reset.
00123     boost::mutex::scoped_lock l(*(this->mutex));
00124     for( unsigned int i=0; i<2 ; ++i)
00125       this->unimportant_data_queue.push(TACTILE_SENSOR_TYPE_RESET_COMMAND);
00126     return true;
00127   }
00128 
00129   //Only to ensure that the template class is compiled for the types we are interested in
00130   template class SensorUpdater<ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND>;
00131   template class SensorUpdater<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>;
00132   template class SensorUpdater<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00133 }
00134 
00135 
00136 
00137 
00138 /* For the emacs weenies in the crowd.
00139 Local Variables:
00140    c-basic-offset: 2
00141 End:
00142 */


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