motor_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  MotorUpdater<CommandType>::MotorUpdater(std::vector<UpdateConfig> update_configs_vector,
38  : GenericUpdater<CommandType>(update_configs_vector, update_state), even_motors(1)
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  {
54  // First we ask for the next data we want to receive
55  if (even_motors)
56  {
57  even_motors = 0;
58  }
59  else
60  {
61  even_motors = 1;
62  this->which_data_to_request++;
63 
64  if (this->which_data_to_request >= this->initialization_configs_vector.size())
65  {
66  this->which_data_to_request = 0;
67  }
68  }
69 
70  command->which_motors = even_motors;
71 
72  // initialization data
73  command->from_motor_data_type =
74  static_cast<FROM_MOTOR_DATA_TYPE>(
75  this->initialization_configs_vector[this->which_data_to_request].what_to_update);
76  ROS_DEBUG_STREAM("Updating initialization data type: " << command->from_motor_data_type << " | [" <<
77  this->which_data_to_request << "/" << this->initialization_configs_vector.size() << "] ");
78  }
79  else
80  {
81  // For the last message sent when a change of update_state happens
82  // (after that we use build_command instead of build_init_command)
83  // we use the first important message and ask it to the even motors (0)
84  // This is to avoid sending a random command
85  command->which_motors = 0;
86  command->from_motor_data_type =
87  static_cast<FROM_MOTOR_DATA_TYPE>(this->important_update_configs_vector[0].what_to_update);
88  ROS_DEBUG_STREAM("Updating important data type: " << command->from_motor_data_type << " | [" <<
89  this->which_data_to_request << "/" << this->important_update_configs_vector.size() << "] ");
90  }
91 
92  this->mutex->unlock();
93 
94  return this->update_state;
95  }
96 
97  template<class CommandType>
99  {
100  if (!this->mutex->try_lock())
101  {
102  return this->update_state;
103  }
104 
106  // First we ask for the next data we want to receive
107  if (even_motors)
108  {
109  even_motors = 0;
110  }
111  else
112  {
113  even_motors = 1;
114  this->which_data_to_request++;
115 
116  if (this->which_data_to_request >= this->important_update_configs_vector.size())
117  {
118  this->which_data_to_request = 0;
119  }
120  }
121 
122  command->which_motors = even_motors;
123 
124  if (!this->unimportant_data_queue.empty())
125  {
126  // an unimportant data is available
127  command->from_motor_data_type = static_cast<FROM_MOTOR_DATA_TYPE>(this->unimportant_data_queue.front());
128  this->unimportant_data_queue.pop();
129 
130  ROS_DEBUG_STREAM("Updating unimportant data type: " << command->from_motor_data_type << " | queue size: " <<
131  this->unimportant_data_queue.size());
132  }
133  else
134  {
135  // important data to update as often as possible
136  command->from_motor_data_type =
137  static_cast<FROM_MOTOR_DATA_TYPE>(
138  this->important_update_configs_vector[this->which_data_to_request].what_to_update);
139  ROS_DEBUG_STREAM("Updating important data type: " << command->from_motor_data_type << " | [" <<
140  this->which_data_to_request << "/" << this->important_update_configs_vector.size() << "] ");
141  }
142 
143  this->mutex->unlock();
144 
145  return this->update_state;
146  }
147 
148  // Only to ensure that the template class is compiled for the types we are interested in
149  template
151 
152  template
154 
155  template
157 } // namespace generic_updater
158 
159 
160 
161 /* For the emacs weenies in the crowd.
162  Local Variables:
163  c-basic-offset: 2
164  End:
165 */
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
operation_mode::device_update_state::DeviceUpdateState build_init_command(CommandType *command)
boost::shared_ptr< boost::mutex > mutex
operation_mode::device_update_state::DeviceUpdateState build_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.
MotorUpdater(std::vector< UpdateConfig > update_configs_vector, operation_mode::device_update_state::DeviceUpdateState update_state)
FROM_MOTOR_DATA_TYPE


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