sr_motor_robot_lib.cpp
Go to the documentation of this file.
1 
28 #include <string>
29 #include <vector>
30 #include <utility>
31 #include <boost/foreach.hpp>
32 
33 #include <sys/time.h>
34 #include <cstdlib>
35 
36 #include <ros/ros.h>
37 
38 #include <controller_manager_msgs/ListControllers.h>
39 
40 #define SERIOUS_ERROR_FLAGS PALM_0200_EDC_SERIOUS_ERROR_FLAGS
41 #define error_flag_names palm_0200_edc_error_flag_names
42 
43 using std::vector;
44 using std::string;
45 using std::pair;
46 using std::ostringstream;
47 using sr_actuator::SrMotorActuator;
54 using boost::shared_ptr;
55 using boost::static_pointer_cast;
56 
57 namespace shadow_robot
58 {
59 
60  template<class StatusType, class CommandType>
61  SrMotorRobotLib<StatusType, CommandType>::SrMotorRobotLib(hardware_interface::HardwareInterface *hw,
62  ros::NodeHandle nh, ros::NodeHandle nhtilde,
63  string device_id, string joint_prefix)
64  : SrRobotLib<StatusType, CommandType>(hw, nh, nhtilde, device_id, joint_prefix),
65  motor_current_state(operation_mode::device_update_state::INITIALIZATION),
66  config_index(MOTOR_CONFIG_FIRST_VALUE),
67  control_type_changed_flag_(false),
68  change_control_type_(this->nh_tilde.advertiseService("change_control_type",
69  &SrMotorRobotLib::change_control_type_callback_,
70  this)),
71  motor_system_control_server_(
72  this->nh_tilde.advertiseService("change_motor_system_controls",
73  &SrMotorRobotLib::motor_system_controls_callback_,
74  this)),
75  lock_command_sending_(new boost::mutex())
76  {
77  // reading the parameters to check for a specified default control type
78  // using FORCE control if no parameters are set
79  string default_control_mode;
80  this->nh_tilde.template param<string>("default_control_mode", default_control_mode, "FORCE");
81  if (default_control_mode.compare("PWM") == 0)
82  {
83  control_type_.control_type = sr_robot_msgs::ControlType::PWM;
84  ROS_INFO("Using PWM control.");
85  }
86  else
87  {
88  control_type_.control_type = sr_robot_msgs::ControlType::FORCE;
89  ROS_INFO("Using TORQUE control.");
90  }
91 
92 #ifdef DEBUG_PUBLISHER
93  this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const);
94  for (int i = 0; i < this->nb_debug_publishers_const; ++i)
95  {
96  ostringstream ss;
97  ss << "srh/debug_" << i;
98  this->debug_publishers.push_back(this->node_handle.template advertise<std_msgs::Int16>(ss.str().c_str(), 100));
99  }
100 #endif
101  }
102 
103  template<class StatusType, class CommandType>
105  {
106  // read the PIC idle time
107  this->main_pic_idle_time = status_data->idle_time_us;
108  if (status_data->idle_time_us < this->main_pic_idle_time_min)
109  {
110  this->main_pic_idle_time_min = status_data->idle_time_us;
111  }
112 
113  // get the current timestamp
114  struct timeval tv;
115  double timestamp = 0.0;
116  if (gettimeofday(&tv, NULL))
117  ROS_WARN("SrMotorRobotLib: Failed to get system time, timestamp in state will be zero");
118  else
119  {
120  timestamp = static_cast<double>(tv.tv_sec) + static_cast<double>(tv.tv_usec) / 1.0e+6;
121  }
122 
123  // First we read the joints information
124  for (vector<Joint>::iterator joint_tmp = this->joints_vector.begin();
125  joint_tmp != this->joints_vector.end();
126  ++joint_tmp)
127  {
128  if (!joint_tmp->has_actuator)
129  {
130  continue;
131  }
132 
133  SrMotorActuator *motor_actuator = this->get_joint_actuator(joint_tmp);
134 
135  shared_ptr<MotorWrapper> motor_wrapper = static_pointer_cast<MotorWrapper>(joint_tmp->actuator_wrapper);
136 
137  motor_index_full = motor_wrapper->motor_id;
138  motor_actuator->state_.device_id_ = motor_index_full;
139 
140  // Fill in the tactiles.
141  if (this->tactiles != NULL)
142  {
143  motor_actuator->motor_state_.tactiles_ = this->tactiles->get_tactile_data();
144  }
145 
146  this->process_position_sensor_data(joint_tmp, status_data, timestamp);
147 
148  // filter the effort
149  pair<double, double> effort_and_effort_d = joint_tmp->effort_filter.compute(
150  motor_actuator->motor_state_.force_unfiltered_, timestamp);
151  motor_actuator->state_.last_measured_effort_ = effort_and_effort_d.first;
152 
153  // get the remaining information.
154  bool read_motor_info = false;
155 
156  if (status_data->which_motors == 0)
157  {
158  // We sampled the even motor numbers
159  if (motor_index_full % 2 == 0)
160  {
161  read_motor_info = true;
162  }
163  }
164  else
165  {
166  // we sampled the odd motor numbers
167  if (motor_index_full % 2 == 1)
168  {
169  read_motor_info = true;
170  }
171  }
172 
173  // the position of the motor in the message
174  // is different from the motor index:
175  // the motor indexes range from 0 to 19
176  // while the message contains information
177  // for only 10 motors.
178  index_motor_in_msg = motor_index_full / 2;
179 
180  // setting the position of the motor in the message,
181  // we'll print that in the diagnostics.
182  motor_wrapper->msg_motor_id = index_motor_in_msg;
183 
184  // OK now we read the info and add it to the actuator state
185  if (read_motor_info)
186  {
187  read_additional_data(joint_tmp, status_data);
188  }
189  } // end for joint
190 
191  // then we read the tactile sensors information
192  this->update_tactile_info(status_data);
193  } // end update()
194 
195  template<class StatusType, class CommandType>
197  {
198  // Mutual exclusion with the change_control_type service.
199  // We have to wait until the control_type_ variable has been set.
200  boost::mutex::scoped_lock l(*lock_command_sending_);
201 
202  if (control_type_changed_flag_)
203  {
204  if (!change_control_parameters(control_type_.control_type))
205  {
206  ROS_FATAL("Changing control parameters failed. Stopping real time loop to protect the robot.");
207  exit(EXIT_FAILURE);
208  }
209 
210  control_type_changed_flag_ = false;
211  }
212 
213  if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
214  {
215  motor_current_state = motor_updater_->build_init_command(command);
216  }
217  else
218  {
219  // build the motor command
220  motor_current_state = motor_updater_->build_command(command);
221  }
222 
223  // Build the tactile sensors command
224  this->build_tactile_command(command);
225 
227  // Now we chose the command to send to the motor
228  // by default we send a torque demand (we're running
229  // the force control on the motors), but if we have a waiting
230  // configuration, a reset command, or a motor system control
231  // request then we send the configuration
232  // or the reset.
233  if (reconfig_queue.empty() && reset_motors_queue.empty()
234  && motor_system_control_flags_.empty())
235  {
236  // no config to send
237  switch (control_type_.control_type)
238  {
239  case sr_robot_msgs::ControlType::FORCE:
240  command->to_motor_data_type = MOTOR_DEMAND_TORQUE;
241  break;
242  case sr_robot_msgs::ControlType::PWM:
243  command->to_motor_data_type = MOTOR_DEMAND_PWM;
244  break;
245 
246  default:
247  command->to_motor_data_type = MOTOR_DEMAND_TORQUE;
248  break;
249  }
250 
251  // loop on all the joints and update their motor: we're sending commands to all the motors.
252  for (vector<Joint>::iterator joint_tmp = this->joints_vector.begin();
253  joint_tmp != this->joints_vector.end();
254  ++joint_tmp)
255  {
256  if (!joint_tmp->has_actuator)
257  {
258  continue;
259  }
260 
261  shared_ptr<MotorWrapper> motor_wrapper = static_pointer_cast<MotorWrapper>(joint_tmp->actuator_wrapper);
262 
263  if (!this->nullify_demand_)
264  {
265  // We send the computed demand
266  command->motor_data[motor_wrapper->motor_id] = motor_wrapper->actuator->command_.effort_;
267  }
268  else
269  {
270  // We want to send a demand of 0
271  command->motor_data[motor_wrapper->motor_id] = 0;
272  }
273 
274  joint_tmp->actuator_wrapper->actuator->state_.last_commanded_effort_ =
275  joint_tmp->actuator_wrapper->actuator->command_.effort_;
276 
277 #ifdef DEBUG_PUBLISHER
278  // publish the debug values for the given motors.
279  // NB: debug_motor_indexes_and_data is smaller
280  // than debug_publishers.
281  int publisher_index = 0;
282  shared_ptr<pair<int, int> > debug_pair;
283  if (this->debug_mutex.try_lock())
284  {
285  BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data)
286  {
287  if (debug_pair != NULL)
288  {
289  MotorWrapper* actuator_wrapper = static_cast<MotorWrapper*> (joint_tmp->actuator_wrapper.get());
290  // check if we want to publish some data for the current motor
291  if (debug_pair->first == actuator_wrapper->motor_id)
292  {
293  // check if it's the correct data
294  if (debug_pair->second == -1)
295  {
296  this->msg_debug.data = joint_tmp->actuator_wrapper->actuator->command_.effort_;
297  this->debug_publishers[publisher_index].publish(this->msg_debug);
298  }
299  }
300  }
301  publisher_index++;
302  }
303 
304  this->debug_mutex.unlock();
305  } // end try_lock
306 #endif
307  } // end for each joint
308  } // end if reconfig_queue.empty()
309  else
310  {
311  if (!motor_system_control_flags_.empty())
312  {
313  // treat the first waiting system control and remove it from the queue
314  vector<sr_robot_msgs::MotorSystemControls> system_controls_to_send;
315  system_controls_to_send = motor_system_control_flags_.front();
316  motor_system_control_flags_.pop();
317 
318  // set the correct type of command to send to the hand.
319  command->to_motor_data_type = MOTOR_SYSTEM_CONTROLS;
320 
321  for (vector<sr_robot_msgs::MotorSystemControls>::iterator it = system_controls_to_send.begin();
322  it != system_controls_to_send.end();
323  ++it)
324  {
325  int16_t combined_flags = 0;
326  if (it->enable_backlash_compensation)
327  {
329  }
330  else
331  {
333  }
334 
335  if (it->increase_sgl_tracking)
336  {
337  combined_flags |= MOTOR_SYSTEM_CONTROL_SGL_TRACKING_INC;
338  }
339  if (it->decrease_sgl_tracking)
340  {
341  combined_flags |= MOTOR_SYSTEM_CONTROL_SGL_TRACKING_DEC;
342  }
343 
344  if (it->increase_sgr_tracking)
345  {
346  combined_flags |= MOTOR_SYSTEM_CONTROL_SGR_TRACKING_INC;
347  }
348  if (it->decrease_sgr_tracking)
349  {
350  combined_flags |= MOTOR_SYSTEM_CONTROL_SGR_TRACKING_DEC;
351  }
352 
353  if (it->initiate_jiggling)
354  {
355  combined_flags |= MOTOR_SYSTEM_CONTROL_INITIATE_JIGGLING;
356  }
357 
358  if (it->write_config_to_eeprom)
359  {
360  combined_flags |= MOTOR_SYSTEM_CONTROL_EEPROM_WRITE;
361  }
362 
363  command->motor_data[it->motor_id] = combined_flags;
364  }
365  } // end if motor_system_control_flags_.empty
366  else
367  {
368  if (!reset_motors_queue.empty())
369  {
370  // reset the CAN messages counters for the motor we're going to reset.
371  int16_t motor_id = reset_motors_queue.front();
372 
373  for (vector<Joint>::iterator joint_tmp = this->joints_vector.begin();
374  joint_tmp != this->joints_vector.end();
375  ++joint_tmp)
376  {
377  if (!joint_tmp->has_actuator)
378  {
379  continue;
380  }
381 
382  shared_ptr<MotorWrapper> motor_wrapper = static_pointer_cast<MotorWrapper>(joint_tmp->actuator_wrapper);
383  SrMotorActuator *actuator = this->get_joint_actuator(joint_tmp);
384 
385  if (motor_wrapper->motor_id == motor_id)
386  {
387  actuator->motor_state_.can_msgs_transmitted_ = 0;
388  actuator->motor_state_.can_msgs_received_ = 0;
389  }
390  }
391 
392  // we have some reset command waiting.
393  // We'll send all of them
394  command->to_motor_data_type = MOTOR_SYSTEM_RESET;
395 
396  while (!reset_motors_queue.empty())
397  {
398  motor_id = reset_motors_queue.front();
399  reset_motors_queue.pop();
400 
401  // we send the MOTOR_RESET_SYSTEM_KEY
402  // and the motor id (on the bus)
403  crc_unions::union16 to_send;
404  to_send.byte[1] = MOTOR_SYSTEM_RESET_KEY >> 8;
405  if (motor_id > 9)
406  {
407  to_send.byte[0] = motor_id - 10;
408  }
409  else
410  {
411  to_send.byte[0] = motor_id;
412  }
413 
414  command->motor_data[motor_id] = to_send.word;
415  }
416  } // end if reset queue not empty
417  else
418  {
419  if (!reconfig_queue.empty())
420  {
421  // we have a waiting config:
422  // we need to send all the config, finishing by the
423  // CRC. We'll remove the config from the queue only
424  // when the whole config has been sent
425 
426  // the motor data type correspond to the index
427  // in the config array.
428  command->to_motor_data_type = static_cast<TO_MOTOR_DATA_TYPE> (config_index);
429 
430  // convert the motor index to the index of the motor in the message
431  int motor_index = reconfig_queue.front().first;
432 
433  // set the data we want to send to the given motor
434  command->motor_data[motor_index] = reconfig_queue.front().second[config_index].word;
435 
436  // We're now sending the CRC. We need to send the correct CRC to
437  // the motor we updated, and CRC=0 to all the other motors in its
438  // group (odd/even) to tell them to ignore the new
439  // configuration.
440  // Once the config has been transmitted, pop the element
441  // and reset the config_index to the beginning of the
442  // config values
443  if (config_index == static_cast<int> (MOTOR_CONFIG_CRC))
444  {
445  // loop on all the motors and send a CRC of 0
446  // except for the motor we're reconfiguring
447  for (int i = 0; i < NUM_MOTORS; ++i)
448  {
449  if (i != motor_index)
450  {
451  command->motor_data[i] = 0;
452  }
453  }
454 
455  // reset the config_index and remove the configuration
456  // we just sent from the configurations queue
457  reconfig_queue.pop();
458  config_index = MOTOR_CONFIG_FIRST_VALUE;
459  }
460  else
461  {
462  ++config_index;
463  }
464  } // end if reconfig queue not empty
465  } // end else reset_queue.empty
466  } // end else motor_system_control_flags_.empty
467  } // end else reconfig_queue.empty() && reset_queue.empty()
468  }
469 
470  template<class StatusType, class CommandType>
471  void SrMotorRobotLib<StatusType, CommandType>::add_diagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec,
473  {
474  for (vector<Joint>::iterator joint = this->joints_vector.begin();
475  joint != this->joints_vector.end();
476  ++joint)
477  {
478  ostringstream name("");
479  string prefix = this->device_id_.empty() ? this->device_id_ : (this->device_id_ + " ");
480  name << prefix << "SRDMotor " << joint->joint_name;
481  d.name = name.str();
482 
483  if (joint->has_actuator)
484  {
485  shared_ptr<MotorWrapper> actuator_wrapper = static_pointer_cast<MotorWrapper>(joint->actuator_wrapper);
486  SrMotorActuator *actuator = this->get_joint_actuator(joint);
487 
488  if (actuator_wrapper->actuator_ok)
489  {
490  if (actuator_wrapper->bad_data)
491  {
492  d.summary(d.WARN, "WARNING, bad CAN data received");
493 
494  d.clear();
495  d.addf("Motor ID", "%d", actuator_wrapper->motor_id);
496  }
497  else // the data is good
498  {
499  d.summary(d.OK, "OK");
500 
501  d.clear();
502  d.addf("Motor ID", "%d", actuator_wrapper->motor_id);
503  d.addf("Motor ID in message", "%d", actuator_wrapper->msg_motor_id);
504  d.addf("Serial Number", "%d", actuator->motor_state_.serial_number);
505  d.addf("Assembly date", "%d / %d / %d",
506  actuator->motor_state_.assembly_data_day,
507  actuator->motor_state_.assembly_data_month,
508  actuator->motor_state_.assembly_data_year);
509 
510  d.addf("Strain Gauge Left", "%d", actuator->motor_state_.strain_gauge_left_);
511  d.addf("Strain Gauge Right", "%d", actuator->motor_state_.strain_gauge_right_);
512 
513  // if some flags are set
514  ostringstream ss;
515  if (actuator->motor_state_.flags_.size() > 0)
516  {
517  int flags_seriousness = d.OK;
518  pair<string, bool> flag;
519 
520  BOOST_FOREACH(flag, actuator->motor_state_.flags_)
521  {
522  // Serious error flag
523  if (flag.second)
524  {
525  flags_seriousness = d.ERROR;
526  }
527 
528  if (flags_seriousness != d.ERROR)
529  {
530  flags_seriousness = d.WARN;
531  }
532  ss << flag.first << " | ";
533  }
534  d.summary(flags_seriousness, ss.str().c_str());
535  }
536  else
537  {
538  ss << " None";
539  }
540  d.addf("Motor Flags", "%s", ss.str().c_str());
541 
542  d.addf("Measured PWM", "%d", actuator->motor_state_.pwm_);
543  d.addf("Measured Current", "%f", actuator->state_.last_measured_current_);
544  d.addf("Measured Voltage", "%f", actuator->state_.motor_voltage_);
545  d.addf("Measured Effort", "%f", actuator->state_.last_measured_effort_);
546  d.addf("Temperature", "%f", actuator->motor_state_.temperature_);
547 
548  d.addf("Unfiltered position", "%f", actuator->motor_state_.position_unfiltered_);
549  d.addf("Unfiltered force", "%f", actuator->motor_state_.force_unfiltered_);
550 
551  d.addf("Gear Ratio", "%d", actuator->motor_state_.motor_gear_ratio);
552 
553  d.addf("Number of CAN messages received", "%lld", actuator->motor_state_.can_msgs_received_);
554  d.addf("Number of CAN messages transmitted", "%lld", actuator->motor_state_.can_msgs_transmitted_);
555 
556  d.addf("Force control Pterm", "%d", actuator->motor_state_.force_control_pterm);
557  d.addf("Force control Iterm", "%d", actuator->motor_state_.force_control_iterm);
558  d.addf("Force control Dterm", "%d", actuator->motor_state_.force_control_dterm);
559 
560  d.addf("Force control F", "%d", actuator->motor_state_.force_control_f_);
561  d.addf("Force control P", "%d", actuator->motor_state_.force_control_p_);
562  d.addf("Force control I", "%d", actuator->motor_state_.force_control_i_);
563  d.addf("Force control D", "%d", actuator->motor_state_.force_control_d_);
564  d.addf("Force control Imax", "%d", actuator->motor_state_.force_control_imax_);
565  d.addf("Force control Deadband", "%d", actuator->motor_state_.force_control_deadband_);
566  d.addf("Force control Frequency", "%d", actuator->motor_state_.force_control_frequency_);
567 
568  if (actuator->motor_state_.force_control_sign_ == 0)
569  {
570  d.addf("Force control Sign", "+");
571  }
572  else
573  {
574  d.addf("Force control Sign", "-");
575  }
576 
577  d.addf("Last Commanded Effort", "%f", actuator->state_.last_commanded_effort_);
578 
579  d.addf("Encoder Position", "%f", actuator->state_.position_);
580 
581  if (actuator->motor_state_.firmware_modified_)
582  {
583  d.addf("Firmware git revision (server / pic / modified)", "%d / %d / True",
584  actuator->motor_state_.server_firmware_git_revision_,
585  actuator->motor_state_.pic_firmware_git_revision_);
586  }
587  else
588  {
589  d.addf("Firmware git revision (server / pic / modified)", "%d / %d / False",
590  actuator->motor_state_.server_firmware_git_revision_,
591  actuator->motor_state_.pic_firmware_git_revision_);
592  }
593  }
594  }
595  else
596  {
597  d.summary(d.ERROR, "Motor error");
598  d.clear();
599  d.addf("Motor ID", "%d", actuator_wrapper->motor_id);
600  }
601  }
602  else
603  {
604  d.summary(d.OK, "No motor associated to this joint");
605  d.clear();
606  }
607  vec.push_back(d);
608  } // end for each joints
609  }
610 
611  template<class StatusType, class CommandType>
613  StatusType *status_data)
614  {
615  if (!joint_tmp->has_actuator)
616  {
617  return;
618  }
619 
620  // check the masks to see if the CAN messages arrived to the motors
621  // the flag should be set to 1 for each motor
622  joint_tmp->actuator_wrapper->actuator_ok = sr_math_utils::is_bit_mask_index_true(
623  status_data->which_motor_data_arrived,
624  motor_index_full);
625 
626  // check the masks to see if a bad CAN message arrived
627  // the flag should be 0
628  joint_tmp->actuator_wrapper->bad_data = sr_math_utils::is_bit_mask_index_true(
629  status_data->which_motor_data_had_errors,
630  index_motor_in_msg);
631 
632  crc_unions::union16 tmp_value;
633 
634  if (joint_tmp->actuator_wrapper->actuator_ok && !(joint_tmp->actuator_wrapper->bad_data))
635  {
636  SrMotorActuator *actuator = static_cast<SrMotorActuator *> (joint_tmp->actuator_wrapper->actuator);
637  MotorWrapper *actuator_wrapper = static_cast<MotorWrapper *> (joint_tmp->actuator_wrapper.get());
638 
639 #ifdef DEBUG_PUBLISHER
640  int publisher_index = 0;
641  // publish the debug values for the given motors.
642  // NB: debug_motor_indexes_and_data is smaller
643  // than debug_publishers.
644  shared_ptr<pair<int, int> > debug_pair;
645 
646  if (this->debug_mutex.try_lock())
647  {
648  BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data)
649  {
650  if (debug_pair != NULL)
651  {
652  MotorWrapper* actuator_wrapper = static_cast<MotorWrapper*> (joint_tmp->actuator_wrapper.get());
653 
654  // check if we want to publish some data for the current motor
655  if (debug_pair->first == actuator_wrapper->motor_id)
656  {
657  // if < 0, then we're not asking for a FROM_MOTOR_DATA_TYPE
658  if (debug_pair->second > 0)
659  {
660  // check if it's the correct data
661  if (debug_pair->second == status_data->motor_data_type)
662  {
663  this->msg_debug.data = status_data->motor_data_packet[index_motor_in_msg].misc;
664  this->debug_publishers[publisher_index].publish(this->msg_debug);
665  }
666  }
667  }
668  }
669  publisher_index++;
670  }
671 
672  this->debug_mutex.unlock();
673  } // end try_lock
674 #endif
675 
676  // we received the data and it was correct
677  bool read_torque = true;
678  switch (status_data->motor_data_type)
679  {
680  case MOTOR_DATA_SGL:
681  actuator->motor_state_.strain_gauge_left_ =
682  static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].misc);
683 
684 #ifdef DEBUG_PUBLISHER
685  if (actuator_wrapper->motor_id == 19)
686  {
687  // ROS_ERROR_STREAM("SGL " <<actuator->motor_state_.strain_gauge_left_);
688  this->msg_debug.data = actuator->motor_state_.strain_gauge_left_;
689  this->debug_publishers[0].publish(this->msg_debug);
690  }
691 #endif
692  break;
693  case MOTOR_DATA_SGR:
694  actuator->motor_state_.strain_gauge_right_ =
695  static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].misc);
696 
697 #ifdef DEBUG_PUBLISHER
698  if (actuator_wrapper->motor_id == 19)
699  {
700  // ROS_ERROR_STREAM("SGR " <<actuator->motor_state_.strain_gauge_right_);
701  this->msg_debug.data = actuator->motor_state_.strain_gauge_right_;
702  this->debug_publishers[1].publish(this->msg_debug);
703  }
704 #endif
705  break;
706  case MOTOR_DATA_PWM:
707  actuator->motor_state_.pwm_ =
708  static_cast<int> (static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].misc));
709 
710 #ifdef DEBUG_PUBLISHER
711  if (actuator_wrapper->motor_id == 19)
712  {
713  // ROS_ERROR_STREAM("SGR " <<actuator->motor_state_.strain_gauge_right_);
714  this->msg_debug.data = actuator->motor_state_.pwm_;
715  this->debug_publishers[2].publish(this->msg_debug);
716  }
717 #endif
718  break;
719  case MOTOR_DATA_FLAGS:
720  actuator->motor_state_.flags_ = humanize_flags(status_data->motor_data_packet[index_motor_in_msg].misc);
721  break;
722  case MOTOR_DATA_CURRENT:
723  // we're receiving the current in milli amps
724  actuator->state_.last_measured_current_ =
725  static_cast<double> (static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc))
726  / 1000.0;
727 
728 #ifdef DEBUG_PUBLISHER
729  if (actuator_wrapper->motor_id == 19)
730  {
731  // ROS_ERROR_STREAM("Current " <<actuator->motor_state_.last_measured_current_);
732  this->msg_debug.data = static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc);
733  this->debug_publishers[3].publish(this->msg_debug);
734  }
735 #endif
736  break;
737  case MOTOR_DATA_VOLTAGE:
738  actuator->state_.motor_voltage_ =
739  static_cast<double> (static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc)) /
740  256.0;
741 
742 #ifdef DEBUG_PUBLISHER
743  if (actuator_wrapper->motor_id == 19)
744  {
745  // ROS_ERROR_STREAM("Voltage " <<actuator->motor_state_.motor_voltage_);
746  this->msg_debug.data = static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc);
747  this->debug_publishers[4].publish(this->msg_debug);
748  }
749 #endif
750  break;
752  actuator->motor_state_.temperature_ =
753  static_cast<double> (static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc)) /
754  256.0;
755  break;
757  // those are 16 bits values and will overflow -> we compute the real value.
758  // This needs to be updated faster than the overflowing period (which should be roughly every 30s)
759  actuator->motor_state_.can_msgs_received_ = sr_math_utils::counter_with_overflow(
760  actuator->motor_state_.can_msgs_received_,
761  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
762  break;
764  // those are 16 bits values and will overflow -> we compute the real value.
765  // This needs to be updated faster than the overflowing period (which should be roughly every 30s)
766  actuator->motor_state_.can_msgs_transmitted_ = sr_math_utils::counter_with_overflow(
767  actuator->motor_state_.can_msgs_transmitted_,
768  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
769  break;
770 
772  // We received a slow data:
773  // the slow data type is contained in .torque, while
774  // the actual data is in .misc.
775  // so we won't read torque information from .torque
776  read_torque = false;
777 
778  switch (static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].torque))
779  {
781  actuator->motor_state_.pic_firmware_git_revision_ =
782  static_cast<unsigned int> (
783  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
784  break;
786  actuator->motor_state_.server_firmware_git_revision_ =
787  static_cast<unsigned int> (
788  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
789  break;
791  actuator->motor_state_.firmware_modified_ =
792  static_cast<bool> (
793  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
794  break;
796  actuator->motor_state_.set_serial_number_low(
797  static_cast<unsigned int> (
798  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc)));
799  break;
801  actuator->motor_state_.set_serial_number_high(
802  static_cast<unsigned int> (
803  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc)));
804  break;
806  actuator->motor_state_.motor_gear_ratio =
807  static_cast<unsigned int> (
808  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
809  break;
811  actuator->motor_state_.assembly_data_year =
812  static_cast<unsigned int> (
813  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
814  break;
816  actuator->motor_state_.assembly_data_month =
817  static_cast<unsigned int> (
818  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc)
819  >> 8);
820  actuator->motor_state_.assembly_data_day =
821  static_cast<unsigned int> (
822  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc)
823  && 0x00FF);
824  break;
826  actuator->motor_state_.force_control_f_ =
827  static_cast<unsigned int> (
828  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
829  break;
831  actuator->motor_state_.force_control_p_ =
832  static_cast<unsigned int> (
833  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
834  break;
836  actuator->motor_state_.force_control_i_ =
837  static_cast<unsigned int> (
838  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
839  break;
841  actuator->motor_state_.force_control_d_ =
842  static_cast<unsigned int> (
843  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
844  break;
846  actuator->motor_state_.force_control_imax_ =
847  static_cast<unsigned int> (
848  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
849  break;
851  tmp_value.word = status_data->motor_data_packet[index_motor_in_msg].misc;
852  actuator->motor_state_.force_control_deadband_ = static_cast<int> (tmp_value.byte[0]);
853  actuator->motor_state_.force_control_sign_ = static_cast<int> (tmp_value.byte[1]);
854  break;
856  actuator->motor_state_.force_control_frequency_ =
857  static_cast<unsigned int> (
858  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc));
859  break;
860 
861  default:
862  break;
863  }
864  break;
865 
867  actuator->motor_state_.can_error_counters =
868  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc);
869  break;
870  case MOTOR_DATA_PTERM:
871  actuator->motor_state_.force_control_pterm =
872  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc);
873  break;
874  case MOTOR_DATA_ITERM:
875  actuator->motor_state_.force_control_iterm =
876  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc);
877  break;
878  case MOTOR_DATA_DTERM:
879  actuator->motor_state_.force_control_dterm =
880  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].misc);
881  break;
882 
883  default:
884  break;
885  }
886 
887  if (read_torque)
888  {
889  actuator->motor_state_.force_unfiltered_ =
890  static_cast<double> (static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].torque));
891 
892 #ifdef DEBUG_PUBLISHER
893  if (actuator_wrapper->motor_id == 19)
894  {
895  this->msg_debug.data = static_cast<int16s> (status_data->motor_data_packet[index_motor_in_msg].torque);
896  this->debug_publishers[5].publish(this->msg_debug);
897  }
898 #endif
899  }
900 
901  // Check the message to see if everything has already been received
902  if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
903  {
904  if (motor_data_checker->check_message(
905  joint_tmp, status_data->motor_data_type,
906  static_cast<int16u> (status_data->motor_data_packet[index_motor_in_msg].torque)))
907  {
908  motor_updater_->update_state = operation_mode::device_update_state::OPERATION;
910 
911  ROS_INFO("All motors were initialized.");
912  }
913  }
914  }
915  }
916 
917  template<class StatusType, class CommandType>
918  void SrMotorRobotLib<StatusType, CommandType>::calibrate_joint(vector<Joint>::iterator joint_tmp,
919  StatusType *status_data)
920  {
921  SrMotorActuator *actuator = get_joint_actuator(joint_tmp);
922 
923  actuator->motor_state_.raw_sensor_values_.clear();
924  actuator->motor_state_.calibrated_sensor_values_.clear();
925 
926  if (joint_tmp->joint_to_sensor.calibrate_after_combining_sensors)
927  {
928  // first we combine the different sensors and then we
929  // calibrate the value we obtained. This is used for
930  // some compound sensors ( THJ5 = cal(THJ5A + THJ5B))
931  double raw_position = 0.0;
932  // when combining the values, we use the coefficient imported
933  // from the sensor_to_joint.yaml file (in sr_edc_launch/config)
934 
935  BOOST_FOREACH(PartialJointToSensor joint_to_sensor, joint_tmp->joint_to_sensor.joint_to_sensor_vector)
936  {
937  int tmp_raw = status_data->sensors[joint_to_sensor.sensor_id];
938  actuator->motor_state_.raw_sensor_values_.push_back(tmp_raw);
939  raw_position += static_cast<double> (tmp_raw) * joint_to_sensor.coeff;
940  }
941 
942  // and now we calibrate
943  this->calibration_tmp = this->calibration_map.find(joint_tmp->joint_name);
944  actuator->motor_state_.position_unfiltered_ = this->calibration_tmp->compute(static_cast<double> (raw_position));
945  }
946  else
947  {
948  // we calibrate the different sensors first and we combine the calibrated
949  // values. This is used in the joint 0s for example ( J0 = cal(J1)+cal(J2) )
950  double calibrated_position = 0.0;
951  PartialJointToSensor joint_to_sensor;
952  string sensor_name;
953 
954  ROS_DEBUG_STREAM("Combining actuator " << joint_tmp->joint_name);
955 
956  for (unsigned int index_joint_to_sensor = 0;
957  index_joint_to_sensor < joint_tmp->joint_to_sensor.joint_to_sensor_vector.size();
958  ++index_joint_to_sensor)
959  {
960  joint_to_sensor = joint_tmp->joint_to_sensor.joint_to_sensor_vector[index_joint_to_sensor];
961  sensor_name = joint_tmp->joint_to_sensor.sensor_names[index_joint_to_sensor];
962 
963  // get the raw position
964  int raw_pos = status_data->sensors[joint_to_sensor.sensor_id];
965  // push the new raw values
966  actuator->motor_state_.raw_sensor_values_.push_back(raw_pos);
967 
968  double tmp_cal_value;
969  bool joint_coupled = this->check_if_joint_coupled(joint_tmp->joint_name);
970  // calibrate and then combine
971  if (joint_coupled)
972  {
973  CoupledJoint *current_joint = &(this->coupled_calibration_map.at(joint_tmp->joint_name));
974  double xyi[2*1];
975  double zi[1];
976  int ni = 1;
977 
978  int raw_pos_2nd_joint =
979  status_data->sensors[
980  this->joints_vector[this->find_joint_by_name(
981  current_joint->sibling_name_)].joint_to_sensor.joint_to_sensor_vector[0].sensor_id];
982  xyi[0] = static_cast<double>(raw_pos);
983  xyi[1] = static_cast<double>(raw_pos_2nd_joint);
984  pwl_interp_2d_scattered_value(current_joint->total_points_, &(current_joint->raw_values_coupled_)[0],
985  &(current_joint->calibrated_values_)[0], current_joint->element_num_,
986  &(current_joint->triangle_)[0], &(current_joint->element_neighbor_)[0],
987  ni, xyi, zi);
988  tmp_cal_value = zi[0];
989  }
990  else
991  {
992  this->calibration_tmp = this->calibration_map.find(sensor_name);
993  tmp_cal_value = this->calibration_tmp->compute(static_cast<double> (raw_pos));
994  }
995  // push the new calibrated values.
996  actuator->motor_state_.calibrated_sensor_values_.push_back(tmp_cal_value);
997 
998  calibrated_position += tmp_cal_value * joint_to_sensor.coeff;
999 
1000  ROS_DEBUG_STREAM(" -> " << sensor_name << " raw = " << raw_pos << " calibrated = " << calibrated_position);
1001  }
1002  actuator->motor_state_.position_unfiltered_ = calibrated_position;
1003  ROS_DEBUG_STREAM(" => " << actuator->motor_state_.position_unfiltered_);
1004  }
1005  } // end calibrate_joint()
1006 
1007  template<class StatusType, class CommandType>
1009  {
1010  if (this->coupled_calibration_map.find(joint_name) == this->coupled_calibration_map.end())
1011  {
1012  return false;
1013  }
1014  return true;
1015  }
1016 
1017  template<class StatusType, class CommandType>
1019  StatusType *status_data, double timestamp)
1020  {
1021  SrMotorActuator *actuator = get_joint_actuator(joint_tmp);
1022 
1023  // calibrate the joint and update the position.
1024  calibrate_joint(joint_tmp, status_data);
1025 
1026  // filter the position and velocity
1027  pair<double, double> pos_and_velocity = joint_tmp->pos_filter.compute(actuator->motor_state_.position_unfiltered_,
1028  timestamp);
1029  // reset the position to the filtered value
1030  actuator->state_.position_ = pos_and_velocity.first;
1031  // set the velocity to the filtered velocity
1032  actuator->state_.velocity_ = pos_and_velocity.second;
1033  }
1034 
1035  template<class StatusType, class CommandType>
1036  vector<pair<string, bool> > SrMotorRobotLib<StatusType, CommandType>::humanize_flags(int flag)
1037  {
1038  vector<pair<string, bool> > flags;
1039 
1040  // 16 is the number of flags
1041  for (unsigned int i = 0; i < 16; ++i)
1042  {
1043  pair<string, bool> new_flag;
1044  // if the flag is set add the name
1045  if (sr_math_utils::is_bit_mask_index_true(flag, i))
1046  {
1047  if (sr_math_utils::is_bit_mask_index_true(SERIOUS_ERROR_FLAGS, i))
1048  {
1049  new_flag.second = true;
1050  }
1051  else
1052  {
1053  new_flag.second = false;
1054  }
1055 
1056  new_flag.first = error_flag_names[i];
1057  flags.push_back(new_flag);
1058  }
1059  }
1060  return flags;
1061  }
1062 
1063  template<class StatusType, class CommandType>
1065  int motor_index, int max_pwm, int sg_left, int sg_right, int f, int p, int i, int d,
1066  int imax, int deadband, int sign, int torque_limit, int torque_limiter_gain)
1067  {
1068  ROS_INFO_STREAM("Setting new pid values for motor" << motor_index <<
1069  ": max_pwm=" << max_pwm <<
1070  " sgleftref=" << sg_left <<
1071  " sgrightref=" << sg_right <<
1072  " f=" << f <<
1073  " p=" << p <<
1074  " i=" << i <<
1075  " d=" << d <<
1076  " imax=" << imax <<
1077  " deadband=" << deadband <<
1078  " sign=" << sign <<
1079  " torque_limit=" << torque_limit <<
1080  " torque_limiter_gain=" << torque_limiter_gain);
1081 
1082  // the vector is of the size of the TO_MOTOR_DATA_TYPE enum.
1083  // the value of the element at a given index is the value
1084  // for the given MOTOR_CONFIG.
1085  vector<crc_unions::union16> full_config(MOTOR_CONFIG_CRC + 1);
1086  crc_unions::union16 value;
1087 
1088  value.word = max_pwm;
1089  full_config.at(MOTOR_CONFIG_MAX_PWM) = value;
1090 
1091  value.byte[0] = sg_left;
1092  value.byte[1] = sg_right;
1093  full_config.at(MOTOR_CONFIG_SG_REFS) = value;
1094 
1095  value.word = f;
1096  full_config.at(MOTOR_CONFIG_F) = value;
1097 
1098  value.word = p;
1099  full_config.at(MOTOR_CONFIG_P) = value;
1100 
1101  value.word = i;
1102  full_config.at(MOTOR_CONFIG_I) = value;
1103 
1104  value.word = d;
1105  full_config.at(MOTOR_CONFIG_D) = value;
1106 
1107  value.word = imax;
1108  full_config.at(MOTOR_CONFIG_IMAX) = value;
1109 
1110  value.byte[0] = deadband;
1111  value.byte[1] = sign;
1112  full_config.at(MOTOR_CONFIG_DEADBAND_SIGN) = value;
1113  ROS_DEBUG_STREAM("deadband: " << static_cast<int> (static_cast<int8u> (value.byte[0])) << " value: " <<
1114  static_cast<int16u> (value.word));
1115 
1116  value.word = torque_limit;
1117  full_config.at(MOTOR_CONFIG_TORQUE_LIMIT) = value;
1118 
1119  value.word = torque_limiter_gain;
1120  full_config.at(MOTOR_CONFIG_TORQUE_LIMITER_GAIN) = value;
1121 
1122 
1123  // compute crc
1124  crc_result = 0;
1125  for (unsigned int i = MOTOR_CONFIG_FIRST_VALUE; i <= MOTOR_CONFIG_LAST_VALUE; ++i)
1126  {
1127  crc_byte = full_config.at(i).byte[0];
1129 
1130  crc_byte = full_config.at(i).byte[1];
1132  }
1133 
1134  // never send a CRC of 0, send 1 if the
1135  // computed CRC is 0 (0 is a code for
1136  // ignoring the config)
1137  if (crc_result == 0)
1138  {
1139  crc_result = 1;
1140  }
1141  value.word = crc_result;
1142  full_config.at(MOTOR_CONFIG_CRC) = value;
1143 
1144  ForceConfig config;
1145  config.first = motor_index;
1146  config.second = full_config;
1147  // push the new config to the configuration queue
1148  reconfig_queue.push(config);
1149  }
1150 
1151  template<class StatusType, class CommandType>
1153  {
1154  // Create a new MotorUpdater object
1155  motor_updater_ = shared_ptr<MotorUpdater<CommandType> >(
1156  new MotorUpdater<CommandType>(motor_update_rate_configs_vector,
1159  // Initialize the motor data checker
1160  motor_data_checker = shared_ptr<MotorDataChecker>(
1161  new MotorDataChecker(this->joints_vector, motor_updater_->initialization_configs_vector));
1162  }
1163 
1164  template<class StatusType, class CommandType>
1166  sr_robot_msgs::ChangeControlType::Request &request,
1167  sr_robot_msgs::ChangeControlType::Response &response)
1168  {
1169  // querying which we're control type we're using currently.
1170  if (request.control_type.control_type == sr_robot_msgs::ControlType::QUERY)
1171  {
1172  response.result = control_type_;
1173  return true;
1174  }
1175 
1176  // We're not querying the control type
1177  if ((request.control_type.control_type != sr_robot_msgs::ControlType::PWM) &&
1178  (request.control_type.control_type != sr_robot_msgs::ControlType::FORCE))
1179  {
1180  string ctrl_type_text = "";
1181  if (control_type_.control_type == sr_robot_msgs::ControlType::FORCE)
1182  {
1183  ctrl_type_text = "FORCE";
1184  }
1185  else
1186  {
1187  ctrl_type_text = "PWM";
1188  }
1189 
1190  ROS_ERROR_STREAM(" The value you specified for the control type (" << request.control_type
1191  << ") is incorrect. Using " << ctrl_type_text << " control.");
1192 
1193  response.result = control_type_;
1194  return false;
1195  }
1196 
1197  if (control_type_.control_type != request.control_type.control_type)
1198  {
1199  // Mutual exclusion with the build_command() function.
1200  // We have to wait until the current motor command has been built.
1201  boost::mutex::scoped_lock l(*lock_command_sending_);
1202 
1203  ROS_WARN("Changing control type");
1204 
1205  control_type_ = request.control_type;
1206  // Flag to signal that there has been a change in the value of control_type_ and certain actions are required.
1207  // The flag is set in the callback function of the change_control_type_ service.
1208  // The flag is checked in build_command() and the necessary actions are taken there.
1209  // These actions involve calling services in the controller manager and all the active controllers. This is the
1210  // reason why we don't do it directly in the callback function. As we use a single thread to serve the callbacks,
1211  // doing so would cause a deadlock, thus we do it in the realtime loop thread instead.
1212  control_type_changed_flag_ = true;
1213  }
1214 
1215  response.result = control_type_;
1216  return true;
1217  }
1218 
1219  template<class StatusType, class CommandType>
1221  {
1222  bool success = true;
1223  string env_variable;
1224  string param_value;
1225 
1226  if (control_type == sr_robot_msgs::ControlType::PWM)
1227  {
1228  env_variable = "PWM_CONTROL=1";
1229  param_value = "PWM";
1230  }
1231  else
1232  {
1233  env_variable = "PWM_CONTROL=0";
1234  param_value = "FORCE";
1235  }
1236 
1237  string arguments = "";
1238 
1239  // Read the hand_id prefix from the parameter server
1240  // The hand_id will be passed as an argument to the sr_edc_default_controllers.launch
1241  // so that the parameters in it will be read from the correct files
1242  string hand_id = "";
1243  this->nodehandle_.template param<string>("hand_id", hand_id, "");
1244  ROS_DEBUG("hand_id: %s", hand_id.c_str());
1245  arguments += " hand_id:=" + hand_id;
1246  ROS_INFO("arguments: %s", arguments.c_str());
1247 
1248  int result = system((env_variable + " roslaunch sr_ethercat_hand_config sr_edc_default_controllers.launch" +
1249  arguments).c_str());
1250 
1251  if (result == 0)
1252  {
1253  ROS_WARN("New parameters loaded successfully on Parameter Server");
1254 
1255  this->nh_tilde.setParam("default_control_mode", param_value);
1256 
1257  // We need another node handle here, that is at the node's base namespace,
1258  // as the controllers and the controller manager are unique per ethercat loop.
1259  ros::NodeHandle nh;
1260  ros::ServiceClient list_ctrl_client = nh.template serviceClient<controller_manager_msgs::ListControllers>(
1261  "controller_manager/list_controllers");
1262  controller_manager_msgs::ListControllers controllers_list;
1263 
1264  if (list_ctrl_client.call(controllers_list))
1265  {
1266  for (unsigned int i = 0; i < controllers_list.response.controller.size(); ++i)
1267  {
1268  if (ros::service::exists(controllers_list.response.controller[i].name + "/reset_gains", false))
1269  {
1270  ros::ServiceClient reset_gains_client = nh.template serviceClient<std_srvs::Empty>(
1271  controllers_list.response.controller[i].name + "/reset_gains");
1272  std_srvs::Empty empty_message;
1273  if (!reset_gains_client.call(empty_message))
1274  {
1276  "Failed to reset gains for controller: " << controllers_list.response.controller[i].name);
1277  return false;
1278  }
1279  }
1280  }
1281  }
1282  else
1283  {
1284  return false;
1285  }
1286  }
1287  else
1288  {
1289  return false;
1290  }
1291 
1292  return success;
1293  }
1294 
1295  template<class StatusType, class CommandType>
1297  sr_robot_msgs::ChangeMotorSystemControls::Request &request,
1298  sr_robot_msgs::ChangeMotorSystemControls::Response &response)
1299  {
1300  vector<sr_robot_msgs::MotorSystemControls> tmp_motor_controls;
1301 
1302  response.result = sr_robot_msgs::ChangeMotorSystemControls::Response::SUCCESS;
1303  bool no_motor_id_out_of_range = true;
1304 
1305  for (unsigned int i = 0; i < request.motor_system_controls.size(); ++i)
1306  {
1307  if (request.motor_system_controls[i].motor_id >= NUM_MOTORS ||
1308  request.motor_system_controls[i].motor_id < 0)
1309  {
1310  response.result = sr_robot_msgs::ChangeMotorSystemControls::Response::MOTOR_ID_OUT_OF_RANGE;
1311  no_motor_id_out_of_range = false;
1312  }
1313  else
1314  {
1315  // only pushes the demands with a correct motor_id
1316  tmp_motor_controls.push_back(request.motor_system_controls[i]);
1317  }
1318  }
1319 
1320  // add the request to the queue if it's not empty
1321  if (tmp_motor_controls.size() > 0)
1322  {
1323  motor_system_control_flags_.push(tmp_motor_controls);
1324  }
1325 
1326  return no_motor_id_out_of_range;
1327  }
1328 
1329  template<class StatusType, class CommandType>
1331  {
1332  int index = 0;
1333  for (vector<Joint>::iterator joint = this->joints_vector.begin();
1334  joint != this->joints_vector.end();
1335  ++joint)
1336  {
1337  if (joint->joint_name.find(joint_name) != std::string::npos)
1338  {
1339  return index;
1340  }
1341  ++index;
1342  }
1343  ROS_ERROR("Could not find joint with name: %s", joint_name.c_str());
1344  return -1;
1345  }
1346 
1347  // Only to ensure that the template class is compiled for the types we are interested in
1348  template
1350 
1351  template
1353 
1354  template
1356 } // namespace shadow_robot
1357 
1358 /* For the emacs weenies in the crowd.
1359  Local Variables:
1360  c-basic-offset: 2
1361  End:
1362 */
std::vector< int > element_neighbor_
unsigned short int16u
MOTOR_SLOW_DATA_CONTROLLER_P
MOTOR_SLOW_DATA_CONTROLLER_FREQUENCY
MOTOR_DATA_SGR
#define ROS_FATAL(...)
#define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_DEC
MOTOR_SYSTEM_CONTROLS
MOTOR_SLOW_DATA_CONTROLLER_D
MOTOR_DATA_PTERM
MOTOR_SLOW_DATA_CONTROLLER_I
#define NUM_MOTORS
MOTOR_CONFIG_LAST_VALUE
MOTOR_SLOW_DATA_CONTROLLER_F
std::pair< int, std::vector< crc_unions::union16 > > ForceConfig
void summary(unsigned char lvl, const std::string s)
#define SERIOUS_ERROR_FLAGS
MOTOR_SLOW_DATA_CONTROLLER_DEADSIGN
ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function< bool(const typename ActionSpec::_action_goal_type::_goal_type &, typename ActionSpec::_action_result_type::_result_type &result)> service_cb)
#define MOTOR_SYSTEM_CONTROL_INITIATE_JIGGLING
bool call(MReq &req, MRes &res)
MOTOR_SLOW_DATA_SVN_REVISION
#define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_DEC
MOTOR_CONFIG_I
MOTOR_CONFIG_TORQUE_LIMIT
SrMotorRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde, std::string device_id, std::string joint_prefix)
MOTOR_CONFIG_SG_REFS
MOTOR_DATA_SGL
MOTOR_CONFIG_CRC
MOTOR_DATA_CURRENT
#define ROS_WARN(...)
MOTOR_DATA_ITERM
MOTOR_DATA_CAN_NUM_RECEIVED
void addf(const std::string &key, const char *format,...)
ros_ethercat_model::Actuator * actuator
MOTOR_SLOW_DATA_ASSEMBLY_DATE_YYYY
MOTOR_SLOW_DATA_CONTROLLER_IMAX
#define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_ENABLE
#define MOTOR_SYSTEM_CONTROL_SGR_TRACKING_INC
MOTOR_CONFIG_IMAX
MOTOR_SLOW_DATA_ASSEMBLY_DATE_MMDD
#define MOTOR_SYSTEM_CONTROL_BACKLASH_COMPENSATION_DISABLE
MOTOR_CONFIG_MAX_PWM
#define ROS_INFO(...)
MOTOR_DATA_VOLTAGE
MOTOR_CONFIG_DEADBAND_SIGN
MOTOR_DATA_CAN_ERROR_COUNTERS
MOTOR_CONFIG_P
MOTOR_SLOW_DATA_SVN_MODIFIED
TO_MOTOR_DATA_TYPE
MOTOR_SLOW_DATA_SERIAL_NUMBER_HIGH
MOTOR_DATA_CAN_NUM_TRANSMITTED
#define MOTOR_SYSTEM_CONTROL_SGL_TRACKING_INC
MOTOR_DATA_FLAGS
#define ROS_DEBUG_STREAM(args)
MOTOR_SLOW_DATA_SERIAL_NUMBER_LOW
#define INSERT_CRC_CALCULATION_HERE
MOTOR_SYSTEM_RESET
MOTOR_CONFIG_TORQUE_LIMITER_GAIN
#define MOTOR_SYSTEM_RESET_KEY
std::vector< double > calibrated_values_
#define ROS_INFO_STREAM(args)
MOTOR_DEMAND_TORQUE
MOTOR_SLOW_DATA_GEAR_RATIO
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
#define error_flag_names
MOTOR_DATA_PWM
MOTOR_DATA_SLOW_MISC
#define ROS_ERROR_STREAM(args)
#define MOTOR_SYSTEM_CONTROL_EEPROM_WRITE
MOTOR_CONFIG_FIRST_VALUE
MOTOR_CONFIG_F
std::vector< int > triangle_
#define ROS_ERROR(...)
std::vector< double > raw_values_coupled_
signed short int16s
MOTOR_DATA_DTERM
MOTOR_CONFIG_D
MOTOR_DEMAND_PWM
MOTOR_SLOW_DATA_SVN_SERVER_REVISION
MOTOR_DATA_TEMPERATURE
#define ROS_DEBUG(...)


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