sensor_updater.cpp

Go to the documentation of this file.

#include "sr_robot_lib/sensor_updater.hpp"
#include <boost/foreach.hpp>
#include <iostream>

namespace generic_updater
{
  SensorUpdater::SensorUpdater(std::vector<UpdateConfig> update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
    : GenericUpdater(update_configs_vector, update_state)
  {
  }

  SensorUpdater::~SensorUpdater()
  {
  }

  operation_mode::device_update_state::DeviceUpdateState SensorUpdater::build_init_command(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND* command)
  {
    if(!mutex->try_lock())
      return update_state;

    if (update_state == operation_mode::device_update_state::INITIALIZATION)
    {
      if(initialization_configs_vector.size()>0)
      {
        // First we ask for the next data we want to receive

        which_data_to_request ++;

        if( which_data_to_request >= initialization_configs_vector.size() )
          which_data_to_request = 0;

        //initialization data
        command->tactile_data_type = initialization_configs_vector[which_data_to_request].what_to_update;
        ROS_DEBUG_STREAM("Updating sensor initialization data type: "<<command->tactile_data_type << " | ["<<which_data_to_request<<"/"<<initialization_configs_vector.size()<<"] ");
      }
    }
    else
    {
      //For the last message sent when a change of update_state happens (after that we use build_command instead of build_init_command)
      //we use the TACTILE_SENSOR_TYPE_WHICH_SENSORS message, which is supposed to be always implemented
      //This is to avoid sending a random command (initialization_configs_vector is empty at this time)
      ROS_DEBUG_STREAM("Important data size: "<<important_update_configs_vector.size());


      command->tactile_data_type = TACTILE_SENSOR_TYPE_WHICH_SENSORS;
      ROS_DEBUG_STREAM("Updating sensor initialization data type: "<<command->tactile_data_type << " | ["<<which_data_to_request<<"/"<<important_update_configs_vector.size()<<"] ");
    }
    mutex->unlock();

    return update_state;
  }

  operation_mode::device_update_state::DeviceUpdateState SensorUpdater::build_command(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND* command)
  {
    if(!mutex->try_lock())
      return update_state;

    // First we ask for the next data we want to receive

    which_data_to_request ++;

    if( which_data_to_request >= important_update_configs_vector.size() )
      which_data_to_request = 0;

    if(!unimportant_data_queue.empty())
    {
      //an unimportant data is available
      command->tactile_data_type = unimportant_data_queue.front();
      unimportant_data_queue.pop();

      ROS_DEBUG_STREAM("Updating sensor unimportant data type: "<<command->tactile_data_type << " | queue size: "<<unimportant_data_queue.size());
    }
    else
    {
      //important data to update as often as possible
      command->tactile_data_type = important_update_configs_vector[which_data_to_request].what_to_update;
      ROS_DEBUG_STREAM("Updating sensor important data type: "<<command->tactile_data_type << " | ["<<which_data_to_request<<"/"<<important_update_configs_vector.size()<<"] ");
    }

    mutex->unlock();

    return update_state;
  }

  bool SensorUpdater::reset()
  {
    //We need to send the reset command twice in a row to make sure
    // the tactiles are reset.
    boost::mutex::scoped_lock l(*mutex);
    for( unsigned int i=0; i<2 ; ++i)
      unimportant_data_queue.push(TACTILE_SENSOR_TYPE_RESET_COMMAND);
  }
}




/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/