muscle_updater.cpp
Go to the documentation of this file.
1 
29 #include <boost/foreach.hpp>
30 #include <iostream>
31 #include <vector>
32 
33 namespace generic_updater
34 {
35  template<class CommandType>
36  MuscleUpdater<CommandType>::MuscleUpdater(std::vector<UpdateConfig> update_configs_vector,
38  : GenericUpdater<CommandType>(update_configs_vector, update_state)
39  {
40  }
41 
42  template<class CommandType>
44  CommandType *command)
45  {
46  if (!this->mutex->try_lock())
47  {
48  return this->update_state;
49  }
50 
52  && (this->initialization_configs_vector.size() > 0))
53  {
55  // First we ask for the next data we want to receive
56  this->which_data_to_request++;
57 
58  if (this->which_data_to_request >= this->initialization_configs_vector.size())
59  {
60  this->which_data_to_request = 0;
61  }
62 
63  // initialization data
64  command->from_muscle_data_type =
65  static_cast<FROM_MUSCLE_DATA_TYPE>(
66  this->initialization_configs_vector[this->which_data_to_request].what_to_update);
67 
68  ROS_DEBUG_STREAM("Updating initialization data type: " << command->from_muscle_data_type << " | [" <<
69  this->which_data_to_request << "/" << this->initialization_configs_vector.size() << "] ");
70  }
71  else
72  {
74  // For the last message sent when a change of update_state happens
75  // (after that we use build_command instead of build_init_command)
76  // we use the first important message
77  // This is to avoid sending a random command
78  command->from_muscle_data_type =
79  static_cast<FROM_MUSCLE_DATA_TYPE>(this->important_update_configs_vector[0].what_to_update);
80  ROS_DEBUG_STREAM("Updating important data type: " << command->from_muscle_data_type << " | [" <<
81  this->which_data_to_request << "/" << this->important_update_configs_vector.size() << "] ");
82  }
83 
84  this->mutex->unlock();
85 
86  return this->update_state;
87  }
88 
89  template<class CommandType>
91  {
92  if (!this->mutex->try_lock())
93  {
94  return this->update_state;
95  }
96 
98  // First we ask for the next data we want to receive
99  this->which_data_to_request++;
100 
101  if (this->which_data_to_request >= this->important_update_configs_vector.size())
102  {
103  this->which_data_to_request = 0;
104  }
105 
106  if (!this->unimportant_data_queue.empty())
107  {
108  // an unimportant data is available
109  command->from_muscle_data_type = static_cast<FROM_MUSCLE_DATA_TYPE>(this->unimportant_data_queue.front());
110  this->unimportant_data_queue.pop();
111 
112  ROS_DEBUG_STREAM("Updating unimportant data type: " << command->from_muscle_data_type << " | queue size: " <<
113  this->unimportant_data_queue.size());
114  }
115  else
116  {
117  // important data to update as often as possible
118  command->from_muscle_data_type =
119  static_cast<FROM_MUSCLE_DATA_TYPE>(
120  this->important_update_configs_vector[this->which_data_to_request].what_to_update);
121  ROS_DEBUG_STREAM("Updating important data type: " << command->from_muscle_data_type << " | [" <<
122  this->which_data_to_request << "/" << this->important_update_configs_vector.size() << "] ");
123  }
124 
125  this->mutex->unlock();
126 
127  return this->update_state;
128  }
129 
130  // Only to ensure that the template class is compiled for the types we are interested in
131  template
133 } // namespace generic_updater
134 
135 
136 
137 /* For the emacs weenies in the crowd.
138  Local Variables:
139  c-basic-offset: 2
140  End:
141 */
std::queue< int32u, std::list< int32u > > unimportant_data_queue
int which_data_to_request
iterate through the important or initialization data types.
operation_mode::device_update_state::DeviceUpdateState update_state
FROM_MUSCLE_DATA_TYPE
boost::shared_ptr< boost::mutex > mutex
operation_mode::device_update_state::DeviceUpdateState build_init_command(CommandType *command)
std::vector< UpdateConfig > initialization_configs_vector
#define ROS_DEBUG_STREAM(args)
std::vector< UpdateConfig > important_update_configs_vector
Contains all the important data types.
MuscleUpdater(std::vector< UpdateConfig > update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
operation_mode::device_update_state::DeviceUpdateState build_command(CommandType *command)


sr_robot_lib
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:43