sr09.cpp
Go to the documentation of this file.
1 
28 
30 
31 #include <math.h>
32 #include <sstream>
33 #include <string>
34 #include <vector>
35 #include <iomanip>
36 #include <boost/foreach.hpp>
37 #include <std_msgs/Int16.h>
38 #include <fcntl.h>
39 #include <stdio.h>
40 #include <pthread.h>
41 
42 #include <sr_utilities/sr_math_utils.hpp>
43 
44 using std::string;
45 using std::stringstream;
46 using std::vector;
47 
49 
50 #include <boost/static_assert.hpp>
51 
52 namespace is_edc_command_32_bits
53 {
54 // check is the EDC_COMMAND is 32bits on the computer
55 // if not, fails
56  BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
57 } // namespace is_edc_command_32_bits
58 
59 #define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS)
60 #define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND)
61 
62 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
63 
64 #define ETHERCAT_COMMAND_DATA_ADDRESS PALM_0240_ETHERCAT_COMMAND_DATA_ADDRESS
65 #define ETHERCAT_STATUS_DATA_ADDRESS PALM_0240_ETHERCAT_STATUS_DATA_ADDRESS
66 #define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS PALM_0240_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
67 #define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS PALM_0240_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
68 
69 
70 PLUGINLIB_EXPORT_CLASS(SR09, EthercatDevice);
71 
79  : zero_buffer_read(0),
80  cycle_count(0),
81  imu_scale_change_(false)
82 {
83  /*
84  ROS_INFO("There are %d sensors", nb_sensors_const);
85  ROS_INFO( "device_pub_freq_const = %d", device_pub_freq_const );
86  ROS_INFO( "ros_pub_freq_const = %d", ros_pub_freq_const );
87  ROS_INFO( "max_iter_const = %d", max_iter_const );
88  ROS_INFO( "nb_sensors_const = %d", nb_sensors_const );
89  ROS_INFO("nb_publish_by_unpack_const = %d", nb_publish_by_unpack_const );
90  */
91 }
92 
126 void SR09::construct(EtherCAT_SlaveHandler *sh, int &start_address)
127 {
132 
133  ROS_INFO("Finished constructing the SR09 driver");
134 }
135 
139 int SR09::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
140 {
141  int retval = SR0X::initialize(hw, allow_unprogrammed);
142 
143  if (retval != 0)
144  {
145  return retval;
146  }
147  hw_ = static_cast<ros_ethercat_model::RobotState *> (hw);
148 
149  sr_hand_lib = boost::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS,
150  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND> >(
151  new shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS,
152  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND>(hw, nodehandle_, nh_tilde_,
154 
155  ROS_INFO("ETHERCAT_STATUS_DATA_SIZE = %4d bytes", static_cast<int> (ETHERCAT_STATUS_DATA_SIZE));
156  ROS_INFO("ETHERCAT_COMMAND_DATA_SIZE = %4d bytes", static_cast<int> (ETHERCAT_COMMAND_DATA_SIZE));
157  ROS_INFO("ETHERCAT_CAN_BRIDGE_DATA_SIZE = %4d bytes", static_cast<int> (ETHERCAT_CAN_BRIDGE_DATA_SIZE));
158 
159  // initialise the publisher for the extra analog inputs, gyroscope and accelerometer on the palm
161  new realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray>(nodehandle_, "palm_extras", 10));
162 
163 
164  // Debug real time publisher: publishes the raw ethercat data
167 
168 
169  std::string imu_name = device_joint_prefix_ + "imu";
170  imu_state_ = hw_->getImuState(imu_name);
171 
172  imu_gyr_scale_server_ = nodehandle_.advertiseService
173  <sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
174  ("/" + imu_name + "/set_gyr_scale", boost::bind(&SR09::imu_scale_callback_, this, _1, _2, "gyr"));
175  imu_acc_scale_server_ = nodehandle_.advertiseService
176  <sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
177  ("/" + imu_name + "/set_acc_scale", boost::bind(&SR09::imu_scale_callback_, this, _1, _2, "acc"));
178 
179  ros::param::param<int>("/" + imu_name + "/acc_scale", imu_scale_acc_, 0);
180  ros::param::param<int>("/" + imu_name + "/gyr_scale", imu_scale_gyr_, 0);
181 
182  imu_scale_change_ = true;
183 
184  return retval;
185 }
186 
187 bool SR09::imu_scale_callback_(sr_robot_msgs::SetImuScale::Request & request,
188  sr_robot_msgs::SetImuScale::Response & response,
189  const char *which)
190 {
191  if (request.scale == 0 || request.scale == 1 || request.scale == 2)
192  {
193  if (which == "acc")
194  {
195  imu_scale_acc_ = request.scale;
196  }
197  else if (which == "gyr")
198  {
199  imu_scale_gyr_ = request.scale;
200  }
201 
202  imu_scale_change_ = true;
203  return true;
204  }
205  else
206  {
207  ROS_WARN_STREAM("Tried to set illegal value: " << (int) request.scale);
208  return false;
209  }
210 }
211 
212 
219 void SR09::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
220 {
221  diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);
222 
223  stringstream name;
224  string prefix = device_id_.empty() ? device_id_ : (device_id_ + " ");
225  d.name = prefix + "EtherCAT Dual CAN Palm";
226  d.summary(d.OK, "OK");
227  stringstream hwid;
228  hwid << sh_->get_product_code() << "-" << sh_->get_serial();
229  d.hardware_id = hwid.str();
230 
231  d.clear();
232  d.addf("Position", "%02d", sh_->get_ring_position());
233  d.addf("Product Code", "%d", sh_->get_product_code());
234  d.addf("Serial Number", "%d", sh_->get_serial());
235  d.addf("Revision", "%d", sh_->get_revision());
236  d.addf("Counter", "%d", ++counter_);
237 
238  d.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
239  d.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
240  // reset the idle time min to a big number, to get a fresh number on next diagnostic
241  sr_hand_lib->main_pic_idle_time_min = 1000;
242 
243  this->ethercatDiagnostics(d, 2);
244  vec.push_back(d);
245 
246  // Add the diagnostics from the hand
247  sr_hand_lib->add_diagnostics(vec, d);
248 
249  // Add the diagnostics from the tactiles
250  if (sr_hand_lib->tactiles != NULL)
251  {
252  sr_hand_lib->tactiles->add_diagnostics(vec, d);
253  }
254 }
255 
271 void SR09::packCommand(unsigned char *buffer, bool halt, bool reset)
272 {
273  SrEdc::packCommand(buffer, halt, reset);
274 
275  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND *command =
276  reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND *>(buffer);
277  ETHERCAT_CAN_BRIDGE_DATA *message = reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(buffer + ETHERCAT_COMMAND_DATA_SIZE);
278 
279  if (!flashing)
280  {
281  command->EDC_command = EDC_COMMAND_SENSOR_DATA;
282  }
283  else
284  {
285  command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
286  }
287 
288  if (imu_scale_change_)
289  {
290  command->imu_command.command = IMU_COMMAND_SET_SCALE;
291  command->imu_command.argument[0] = imu_scale_acc_;
292  command->imu_command.argument[1] = imu_scale_gyr_;
293  imu_scale_change_ = false;
294  }
295  else
296  {
297  command->imu_command.command = IMU_COMMAND_NONE;
298  }
299 
300  // alternate between even and uneven motors
301  // and ask for the different informations.
302  sr_hand_lib->build_command(command);
303 
304 
305  ROS_DEBUG("Sending command : Type : 0x%02X ; data : 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X "
306  "0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X",
307  command->to_motor_data_type,
308  command->motor_data[0],
309  command->motor_data[1],
310  command->motor_data[2],
311  command->motor_data[3],
312  command->motor_data[4],
313  command->motor_data[5],
314  command->motor_data[6],
315  command->motor_data[7],
316  command->motor_data[8],
317  command->motor_data[9],
318  command->motor_data[10],
319  command->motor_data[11],
320  command->motor_data[12],
321  command->motor_data[13],
322  command->motor_data[14],
323  command->motor_data[15],
324  command->motor_data[16],
325  command->motor_data[17],
326  command->motor_data[18],
327  command->motor_data[19]);
328 
329  build_CAN_message(message);
330 }
331 
334 void SR09::readImu(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS * status_data)
335 {
336  imu_state_->data_.orientation[0] = 0.0; imu_state_->data_.orientation[1] = 0.0;
337  imu_state_->data_.orientation[2] = 0.0; imu_state_->data_.orientation[3] = 1.0;
338 
339  double acc_multiplier = 1 << imu_scale_acc_;
340  double gyr_multiplier = 1 << imu_scale_gyr_;
341 
342  int zero_catch = 0;
343 
344  for (size_t x = 0; x < 2; ++x)
345  {
346  if (status_data->sensors[ACCX + 0] == 0)
347  {
348  ++zero_catch;
349  }
350  if (status_data->sensors[GYRX + 0] == 0)
351  {
352  ++zero_catch;
353  }
354  }
355  if (zero_catch <= 1)
356  {
357  imu_state_->data_.linear_acceleration[0] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCX]);
358  imu_state_->data_.linear_acceleration[1] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCY]);
359  imu_state_->data_.linear_acceleration[2] = acc_multiplier * static_cast<int16s>(status_data->sensors[ACCZ]);
360 
361  imu_state_->data_.angular_velocity[0] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRX]);
362  imu_state_->data_.angular_velocity[1] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRY]);
363  imu_state_->data_.angular_velocity[2] = gyr_multiplier * static_cast<int16s>(status_data->sensors[GYRZ]);
364  }
365  for (size_t x = 0; x < 9; ++x)
366  {
367  imu_state_->data_.linear_acceleration_covariance[x] = 0.0;
368  imu_state_->data_.angular_velocity_covariance[x] = 0.0;
369  imu_state_->data_.orientation_covariance[x] = 0.0;
370  }
371 }
372 
373 
374 
393 bool SR09::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
394 {
395  ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *status_data =
396  reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *>(this_buffer + command_size_);
397  ETHERCAT_CAN_BRIDGE_DATA *can_data = reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(this_buffer + command_size_ +
399  // int16u *status_buffer = (int16u*)status_data;
400  static unsigned int num_rxed_packets = 0;
401 
402  ++num_rxed_packets;
403 
404  readImu(status_data);
405 
406  // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
407  if (debug_publisher->trylock())
408  {
409  debug_publisher->msg_.header.stamp = ros::Time::now();
410 
411  debug_publisher->msg_.sensors.clear();
412  for (unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
413  {
414  debug_publisher->msg_.sensors.push_back(status_data->sensors[i]);
415  }
416 
417  debug_publisher->msg_.motor_data_type.data = static_cast<int> (status_data->motor_data_type);
418  debug_publisher->msg_.which_motors = status_data->which_motors;
419  debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
420  debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;
421 
422  debug_publisher->msg_.motor_data_packet_torque.clear();
423  debug_publisher->msg_.motor_data_packet_misc.clear();
424  for (unsigned int i = 0; i < 10; ++i)
425  {
426  debug_publisher->msg_.motor_data_packet_torque.push_back(status_data->motor_data_packet[i].torque);
427  debug_publisher->msg_.motor_data_packet_misc.push_back(status_data->motor_data_packet[i].misc);
428  }
429 
430  debug_publisher->msg_.tactile_data_type = static_cast<unsigned int>(
431  static_cast<int32u>(status_data->tactile_data_type));
432  debug_publisher->msg_.tactile_data_valid = static_cast<unsigned int> (
433  static_cast<int16u> (status_data->tactile_data_valid));
434  debug_publisher->msg_.tactile.clear();
435  for (unsigned int i = 0; i < 5; ++i)
436  {
437  debug_publisher->msg_.tactile.push_back(
438  static_cast<unsigned int> (static_cast<int16u> (status_data->tactile[i].word[0])));
439  }
440 
441  debug_publisher->msg_.idle_time_us = status_data->idle_time_us;
442 
443  debug_publisher->unlockAndPublish();
444  }
445 
446  if (status_data->EDC_command == EDC_COMMAND_INVALID)
447  {
448  // received empty message: the pic is not writing to its mailbox.
450  float percentage_packet_loss = 100.f * (static_cast<float>(zero_buffer_read) /
451  static_cast<float>(num_rxed_packets));
452 
453  ROS_DEBUG("Reception error detected : %d errors out of %d rxed packets (%2.3f%%) ; idle time %dus",
454  zero_buffer_read, num_rxed_packets, percentage_packet_loss, status_data->idle_time_us);
455  return true;
456  }
457 
458  // We received a coherent message.
459  // Update the library (positions, diagnostics values, actuators, etc...)
460  // with the received information
461  sr_hand_lib->update(status_data);
462 
463  // Now publish the additional data at 100Hz (every 10 cycles)
464  if (cycle_count >= 10)
465  {
466  // publish tactiles if we have them
467  if (sr_hand_lib->tactiles != NULL)
468  {
469  sr_hand_lib->tactiles->publish();
470  }
471 
472  // And we also publish the additional data (accelerometer / gyroscope / analog inputs)
473  std_msgs::Float64MultiArray extra_analog_msg;
474  extra_analog_msg.layout.dim.resize(3);
475  extra_analog_msg.data.resize(3 + 3 + 4);
476  std::vector<double> data;
477 
478  extra_analog_msg.layout.dim[0].label = "accelerometer";
479  extra_analog_msg.layout.dim[0].size = 3;
480  extra_analog_msg.data[0] = status_data->sensors[ACCX];
481  extra_analog_msg.data[1] = status_data->sensors[ACCY];
482  extra_analog_msg.data[2] = status_data->sensors[ACCZ];
483 
484  extra_analog_msg.layout.dim[1].label = "gyrometer";
485  extra_analog_msg.layout.dim[1].size = 3;
486  extra_analog_msg.data[3] = status_data->sensors[GYRX];
487  extra_analog_msg.data[4] = status_data->sensors[GYRY];
488  extra_analog_msg.data[5] = status_data->sensors[GYRZ];
489 
490  extra_analog_msg.layout.dim[2].label = "analog_inputs";
491  extra_analog_msg.layout.dim[2].size = 4;
492  extra_analog_msg.data[6] = status_data->sensors[ANA0];
493  extra_analog_msg.data[7] = status_data->sensors[ANA1];
494  extra_analog_msg.data[8] = status_data->sensors[ANA2];
495  extra_analog_msg.data[9] = status_data->sensors[ANA3];
496 
497  if (extra_analog_inputs_publisher->trylock())
498  {
499  extra_analog_inputs_publisher->msg_ = extra_analog_msg;
500  extra_analog_inputs_publisher->unlockAndPublish();
501  }
502 
503  cycle_count = 0;
504  }
505  ++cycle_count;
506 
507 
508  // If we're flashing, check is the packet has been acked
509  if (flashing & !can_packet_acked)
510  {
511  if (can_data_is_ack(can_data))
512  {
513  can_packet_acked = true;
514  }
515  }
516 
517  return true;
518 }
519 
521 {
522  // Reinitialize motors information
523  sr_hand_lib->reinitialize_motors();
524 }
525 
526 void SR09::get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
527 {
528  // We're using 2 can busses,
529  // if motor id is between 0 and 9, then we're using the can_bus 1
530  // else, we're using the can bus 2.
531  int motor_id_tmp = board_id;
532  if (motor_id_tmp > 9)
533  {
534  motor_id_tmp -= 10;
535  *can_bus = 2;
536  }
537  else
538  {
539  *can_bus = 1;
540  }
541 
542  *board_can_id = motor_id_tmp;
543 }
544 
545 /* For the emacs weenies in the crowd.
546  Local Variables:
547  c-basic-offset: 2
548  End:
549 */
d
unsigned short int16u
ros::NodeHandle nh_tilde_
Definition: sr_edc.h:87
int16_t cycle_count
Definition: sr09.h:120
EDC_COMMAND_SENSOR_DATA
bool imu_scale_change_
Definition: sr09.h:115
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)
Definition: sr0x.cpp:36
void summary(unsigned char lvl, const std::string s)
ros::ServiceServer imu_gyr_scale_server_
Definition: sr09.h:100
ANA0
ACCZ
boost::shared_ptr< shadow_robot::SrMotorHandLib< ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_COMMAND > > sr_hand_lib
Definition: sr09.h:111
virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address)
Construct function, run at startup to set SyncManagers and FMMUs.
Definition: sr09.cpp:126
unsigned int zero_buffer_read
Definition: sr09.h:104
data
ACCX
This is a ROS driver for Shadow Robot #9 EtherCAT product ID.
Definition: sr09.h:57
ros_ethercat_model::RobotState * hw_
Definition: sr09.h:107
#define ETHERCAT_STATUS_DATA_SIZE
Definition: sr09.cpp:59
ros::ServiceServer imu_acc_scale_server_
Definition: sr09.h:101
virtual void packCommand(unsigned char *buffer, bool halt, bool reset)
packs the commands before sending them to the EtherCAT bus
Definition: sr09.cpp:271
virtual void multiDiagnostics(vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
This function gives some diagnostics data.
Definition: sr09.cpp:219
virtual void reinitialize_boards()
This function will call the reinitialization function for the boards attached to the CAN bus...
Definition: sr09.cpp:520
ANA3
int counter_
Definition: sr_edc.h:85
void addf(const std::string &key, const char *format,...)
virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
Definition: sr09.cpp:526
int imu_scale_gyr_
Definition: sr09.h:113
unsigned int int32u
boost::shared_ptr< realtime_tools::RealtimePublisher< sr_robot_msgs::EthercatDebug > > debug_publisher
Debug real time publisher: publishes the raw ethercat data.
Definition: sr09.h:125
BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND)==4)
#define ETHERCAT_COMMAND_DATA_ADDRESS
Definition: sr09.cpp:64
EDC_COMMAND_CAN_DIRECT_MODE
virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address, unsigned int ethercat_command_data_size, unsigned int ethercat_status_data_size, unsigned int ethercat_can_bridge_data_size, unsigned int ethercat_command_data_address, unsigned int ethercat_status_data_address, unsigned int ethercat_can_bridge_data_command_address, unsigned int ethercat_can_bridge_data_status_address)
Construct function, run at startup to set SyncManagers and FMMUs.
Definition: sr_edc.cpp:167
bool can_packet_acked
Definition: sr_edc.h:96
ANA2
ROSLIB_DECL std::string command(const std::string &cmd)
#define ROS_INFO(...)
PLUGINLIB_EXPORT_CLASS(SR09, EthercatDevice)
virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
This functions receives data from the EtherCAT bus.
Definition: sr09.cpp:393
ros::NodeHandle nodehandle_
Definition: sr_edc.h:86
#define IMU_COMMAND_SET_SCALE
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)
Definition: sr09.cpp:139
#define ETHERCAT_COMMAND_DATA_SIZE
Definition: sr09.cpp:60
SR09()
Constructor of the SR09 driver.
Definition: sr09.cpp:78
ACCY
void build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message)
Definition: sr_edc.cpp:615
int imu_scale_acc_
Definition: sr09.h:114
#define IMU_COMMAND_NONE
GYRY
EDC_COMMAND_INVALID
#define ROS_WARN_STREAM(args)
#define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
Definition: sr09.cpp:66
bool can_data_is_ack(ETHERCAT_CAN_BRIDGE_DATA *packet)
This function checks if the can packet in the unpackState() this_buffer is an ACK.
Definition: sr_edc.cpp:668
void readImu(ETHERCAT_DATA_STRUCTURE_0240_PALM_EDC_STATUS *status)
This funcion reads the ethercat status and fills the imu_state with the relevant values.
Definition: sr09.cpp:334
ros_ethercat_model::ImuState * imu_state_
Definition: sr09.h:109
#define ETHERCAT_CAN_BRIDGE_DATA_SIZE
Definition: sr09.cpp:62
boost::shared_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > > extra_analog_inputs_publisher
Extra analog inputs real time publisher (+ accelerometer and gyroscope)
Definition: sr09.h:78
GYRZ
static Time now()
ANA1
std::string device_joint_prefix_
Definition: sr_edc.h:99
std::string device_id_
Definition: sr_edc.h:98
GYRX
EDC_COMMAND
#define ETHERCAT_STATUS_DATA_ADDRESS
Definition: sr09.cpp:65
#define SENSORS_NUM_0220
bool flashing
Definition: sr_edc.h:95
signed short int16s
#define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
Definition: sr09.cpp:67
bool imu_scale_callback_(sr_robot_msgs::SetImuScale::Request &request, sr_robot_msgs::SetImuScale::Response &response, const char *which)
Definition: sr09.cpp:187
#define ROS_DEBUG(...)


sr_edc_ethercat_drivers
Author(s): Ugo Cupcic, Yann Sionneau, Toni Oliver
autogenerated on Mon Feb 28 2022 23:50:53