muscle_updater.cpp
Go to the documentation of this file.
00001 
00029 #include "sr_robot_lib/muscle_updater.hpp"
00030 #include <boost/foreach.hpp>
00031 #include <iostream>
00032 
00033 namespace generic_updater
00034 {
00035   template <class CommandType>
00036   MuscleUpdater<CommandType>::MuscleUpdater(std::vector<UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
00037     : GenericUpdater<CommandType>(update_configs_vector, update_state)
00038   {
00039   }
00040 
00041   template <class CommandType>
00042   MuscleUpdater<CommandType>::~MuscleUpdater()
00043   {
00044   }
00045 
00046   template <class CommandType>
00047   operation_mode::device_update_state::DeviceUpdateState MuscleUpdater<CommandType>::build_init_command(CommandType* command)
00048   {
00049     if(!this->mutex->try_lock())
00050       return this->update_state;
00051 
00052     if ((this->update_state == operation_mode::device_update_state::INITIALIZATION)
00053         && (this->initialization_configs_vector.size() > 0))
00054     {
00056       // First we ask for the next data we want to receive
00057       this->which_data_to_request ++;
00058 
00059       if( this->which_data_to_request >= this->initialization_configs_vector.size() )
00060         this->which_data_to_request = 0;
00061 
00062       //initialization data
00063       command->from_muscle_data_type = static_cast<FROM_MUSCLE_DATA_TYPE>(this->initialization_configs_vector[this->which_data_to_request].what_to_update);
00064       ROS_DEBUG_STREAM("Updating initialization data type: "<<command->from_muscle_data_type << " | ["<<this->which_data_to_request<<"/"<<this->initialization_configs_vector.size()<<"] ");
00065     }
00066     else
00067     {
00068       this->update_state == operation_mode::device_update_state::OPERATION;
00069       //For the last message sent when a change of update_state happens (after that we use build_command instead of build_init_command)
00070       //we use the first important message
00071       //This is to avoid sending a random command
00072       command->from_muscle_data_type = static_cast<FROM_MUSCLE_DATA_TYPE>(this->important_update_configs_vector[0].what_to_update);
00073       ROS_DEBUG_STREAM("Updating important data type: "<<command->from_muscle_data_type << " | ["<<this->which_data_to_request<<"/"<<this->important_update_configs_vector.size()<<"] ");
00074     }
00075 
00076     this->mutex->unlock();
00077 
00078     return this->update_state;
00079   }
00080 
00081   template <class CommandType>
00082   operation_mode::device_update_state::DeviceUpdateState MuscleUpdater<CommandType>::build_command(CommandType* command)
00083   {
00084     if(!this->mutex->try_lock())
00085       return this->update_state;
00086 
00088     // First we ask for the next data we want to receive
00089     this->which_data_to_request ++;
00090 
00091     if( this->which_data_to_request >= this->important_update_configs_vector.size() )
00092       this->which_data_to_request = 0;
00093 
00094     if(!this->unimportant_data_queue.empty())
00095     {
00096       //an unimportant data is available
00097       command->from_muscle_data_type = static_cast<FROM_MUSCLE_DATA_TYPE>(this->unimportant_data_queue.front());
00098       this->unimportant_data_queue.pop();
00099 
00100       ROS_DEBUG_STREAM("Updating unimportant data type: "<<command->from_muscle_data_type << " | queue size: "<<this->unimportant_data_queue.size());
00101     }
00102     else
00103     {
00104       //important data to update as often as possible
00105       command->from_muscle_data_type = static_cast<FROM_MUSCLE_DATA_TYPE>(this->important_update_configs_vector[this->which_data_to_request].what_to_update);
00106       ROS_DEBUG_STREAM("Updating important data type: "<<command->from_muscle_data_type << " | ["<<this->which_data_to_request<<"/"<<this->important_update_configs_vector.size()<<"] ");
00107     }
00108 
00109     this->mutex->unlock();
00110 
00111     return this->update_state;
00112   }
00113 
00114   //Only to ensure that the template class is compiled for the types we are interested in
00115   template class MuscleUpdater<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND>;
00116 }
00117 
00118 
00119 
00120 /* For the emacs weenies in the crowd.
00121    Local Variables:
00122    c-basic-offset: 2
00123    End:
00124 */


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:37:50