motor_data_checker.cpp
Go to the documentation of this file.
1 
27 #include <vector>
28 
29 namespace generic_updater
30 {
31  const double MotorDataChecker::timeout = 5.0;
32 
33  MotorDataChecker::MotorDataChecker(std::vector<shadow_joints::Joint> joints_vector,
34  std::vector<UpdateConfig> initialization_configs_vector)
35  : nh_tilde("~"), update_state(operation_mode::device_update_state::INITIALIZATION), init_max_duration(timeout)
36  {
37  init(joints_vector, initialization_configs_vector);
38  }
39 
41  {
42  for (unsigned int i = 0; i < msg_checkers_.size(); i++)
43  {
44  for (unsigned int j = 0; j < msg_checkers_.at(i).msg_from_motor_checkers.size(); j++)
45  {
46  delete msg_checkers_.at(i).msg_from_motor_checkers.at(j);
47  }
48  }
49  }
50 
51  void MotorDataChecker::init(std::vector<shadow_joints::Joint> joints_vector,
52  std::vector<UpdateConfig> initialization_configs_vector)
53  {
54  // Create a one-shot timer
56  boost::bind(&MotorDataChecker::timer_callback, this, _1), true);
58  msg_checkers_.clear();
59 
60  std::vector<UpdateConfig>::iterator msg_it;
61 
62  for (msg_it = initialization_configs_vector.begin(); msg_it < initialization_configs_vector.end(); msg_it++)
63  {
64  MessageChecker tmp_msg_checker(static_cast<FROM_MOTOR_DATA_TYPE>(msg_it->what_to_update));
65  for (std::vector<shadow_joints::Joint>::iterator joint = joints_vector.begin();
66  joint != joints_vector.end();
67  ++joint)
68  {
69  if (joint->has_actuator)
70  {
72  boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint->actuator_wrapper);
73  if (msg_it->what_to_update == MOTOR_DATA_SLOW_MISC)
74  {
75  tmp_msg_checker.msg_from_motor_checkers.push_back(new SlowMessageFromMotorChecker(motor_wrapper->motor_id));
76  }
77  else
78  {
79  tmp_msg_checker.msg_from_motor_checkers.push_back(new MessageFromMotorChecker(motor_wrapper->motor_id));
80  }
81  }
82  }
83  msg_checkers_.push_back(tmp_msg_checker);
84  }
85  }
86 
87  bool MotorDataChecker::check_message(std::vector<shadow_joints::Joint>::iterator joint_tmp,
88  FROM_MOTOR_DATA_TYPE motor_data_type, int16u motor_slow_data_type)
89  {
90  int index_motor_data_type = 0;
91 
92  index_motor_data_type = find(motor_data_type);
93  if (index_motor_data_type != (-1))
94  {
95  int index_motor_id = 0;
97  boost::static_pointer_cast<shadow_joints::MotorWrapper>(joint_tmp->actuator_wrapper);
98  index_motor_id = msg_checkers_.at(index_motor_data_type).find(motor_wrapper->motor_id);
99  if (index_motor_id != (-1))
100  {
101  if (motor_data_type == MOTOR_DATA_SLOW_MISC)
102  {
103  SlowMessageFromMotorChecker *ptr_tmp_checker =
104  dynamic_cast<SlowMessageFromMotorChecker *>(
105  msg_checkers_.at(index_motor_data_type).msg_from_motor_checkers.at(index_motor_id));
106 
107  if (ptr_tmp_checker != NULL)
108  {
109  ptr_tmp_checker->set_received(static_cast<FROM_MOTOR_SLOW_DATA_TYPE>(motor_slow_data_type));
110  }
111  else
112  {
113  ROS_ERROR_STREAM("Checker conversion failed");
114  }
115  }
116  else
117  {
118  msg_checkers_.at(index_motor_data_type).msg_from_motor_checkers.at(index_motor_id)->set_received();
119  }
120  }
121  else
122  {
123  ROS_ERROR_STREAM("Motor id not found: " << motor_wrapper->motor_id);
124  }
125  }
127  }
128 
130  {
131  std::vector<MessageChecker>::iterator it;
132 
133  for (it = msg_checkers_.begin(); it < msg_checkers_.end(); it++)
134  {
135  std::vector<MessageFromMotorChecker *>::iterator it2;
136 
137  for (it2 = it->msg_from_motor_checkers.begin(); it2 < it->msg_from_motor_checkers.end(); it2++)
138  {
139  if (!(*it2)->get_received())
140  {
141  return false;
142  }
143  }
144  }
145 
146  // all the motors are initialized -> we stop the timeout timer
149  return true;
150  }
151 
153  {
154  for (unsigned int i = 0; i < msg_checkers_.size(); i++)
155  {
156  if (msg_checkers_.at(i).msg_type == motor_data_type)
157  {
158  return i;
159  }
160  }
161  return (-1);
162  }
163 
165  {
167  {
169  ROS_ERROR_STREAM("Motor Initialization Timeout: the static information in the diagnostics may not be uptodate.");
170  }
171  }
172 
173  int MessageChecker::find(int motor_id)
174  {
175  for (unsigned int i = 0; i < msg_from_motor_checkers.size(); i++)
176  {
177  if (msg_from_motor_checkers.at(i)->motor_id_ == motor_id)
178  {
179  return i;
180  }
181  }
182  return (-1);
183  }
184 
187  {
188  for (int i = 0; i <= MOTOR_SLOW_DATA_LAST; i++)
189  {
190  slow_data_received.at(i) = false;
191  }
192  }
193 
195  {
196  if (received_ == false)
197  {
198  // Check the slow data type as received
199  if (slow_data_type > MOTOR_SLOW_DATA_LAST)
200  {
201  ROS_ERROR_STREAM("Received bad slow_data_type: " << slow_data_type << " > " << slow_data_received.size());
202  return;
203  }
204  slow_data_received.at(slow_data_type) = true;
205 
206  // look if every type is received, then change FROM_MOTOR_SLOW_DATA_TYPE general received state accordingly
207  bool checked = true;
208  for (int i = MOTOR_SLOW_DATA_SVN_REVISION; i <= MOTOR_SLOW_DATA_LAST; i++)
209  {
210  checked = checked && slow_data_received.at(i);
211  if (!checked)
212  {
213  break;
214  }
215  }
216  if (checked)
217  {
218  received_ = true;
219  }
220  }
221  }
222 
224  {
225  received_ = true;
226  }
227 
229  {
230  return received_;
231  }
232 } // namespace generic_updater
233 
234 /* For the emacs weenies in the crowd.
235  Local Variables:
236  c-basic-offset: 2
237  End:
238 */
unsigned short int16u
void timer_callback(const ros::TimerEvent &event)
int find(FROM_MOTOR_DATA_TYPE motor_data_type)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void stop()
MOTOR_SLOW_DATA_SVN_REVISION
operation_mode::device_update_state::DeviceUpdateState update_state
MotorDataChecker(std::vector< shadow_joints::Joint > joints_vector, std::vector< UpdateConfig > initialization_configs_vector)
boost::array< bool, MOTOR_SLOW_DATA_LAST+1 > slow_data_received
std::vector< MessageFromMotorChecker * > msg_from_motor_checkers
std::vector< MessageChecker > msg_checkers_
MOTOR_SLOW_DATA_LAST
virtual void set_received(FROM_MOTOR_SLOW_DATA_TYPE slow_data_type)
bool check_message(std::vector< shadow_joints::Joint >::iterator joint_tmp, FROM_MOTOR_DATA_TYPE motor_data_type, int16u motor_slow_data_type)
void init(std::vector< shadow_joints::Joint > joints_vector, std::vector< UpdateConfig > initialization_configs_vector)
MOTOR_DATA_SLOW_MISC
#define ROS_ERROR_STREAM(args)
FROM_MOTOR_SLOW_DATA_TYPE
FROM_MOTOR_DATA_TYPE


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