sr08.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_0230_PALM_EDC_STATUS)
60 #define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND)
61 
62 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
63 
64 #define ETHERCAT_COMMAND_DATA_ADDRESS PALM_0230_ETHERCAT_COMMAND_DATA_ADDRESS
65 #define ETHERCAT_STATUS_DATA_ADDRESS PALM_0230_ETHERCAT_STATUS_DATA_ADDRESS
66 #define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS PALM_0230_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
67 #define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS PALM_0230_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
68 
69 
70 PLUGINLIB_EXPORT_CLASS(SR08, EthercatDevice);
71 
79  : zero_buffer_read(0),
80  cycle_count(0)
81 {
82  /*
83  ROS_INFO("There are %d sensors", nb_sensors_const);
84  ROS_INFO( "device_pub_freq_const = %d", device_pub_freq_const );
85  ROS_INFO( "ros_pub_freq_const = %d", ros_pub_freq_const );
86  ROS_INFO( "max_iter_const = %d", max_iter_const );
87  ROS_INFO( "nb_sensors_const = %d", nb_sensors_const );
88  ROS_INFO("nb_publish_by_unpack_const = %d", nb_publish_by_unpack_const );
89  */
90 }
91 
125 void SR08::construct(EtherCAT_SlaveHandler *sh, int &start_address)
126 {
129 
134 
135  ROS_INFO("Finished constructing the SR08 driver");
136 }
137 
141 int SR08::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
142 {
143  int retval = SR0X::initialize(hw, allow_unprogrammed);
144 
145  if (retval != 0)
146  {
147  return retval;
148  }
149 
150  sr_hand_lib = boost::shared_ptr<shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS,
151  ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND> >(
152  new shadow_robot::SrMotorHandLib<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS,
153  ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND>(hw, nodehandle_, nh_tilde_,
155 
156  ROS_INFO("ETHERCAT_STATUS_DATA_SIZE = %4d bytes", static_cast<int> (ETHERCAT_STATUS_DATA_SIZE));
157  ROS_INFO("ETHERCAT_COMMAND_DATA_SIZE = %4d bytes", static_cast<int> (ETHERCAT_COMMAND_DATA_SIZE));
158  ROS_INFO("ETHERCAT_CAN_BRIDGE_DATA_SIZE = %4d bytes", static_cast<int> (ETHERCAT_CAN_BRIDGE_DATA_SIZE));
159 
160  // initialise the publisher for the extra analog inputs, gyroscope and accelerometer on the palm
162  new realtime_tools::RealtimePublisher<std_msgs::Float64MultiArray>(nodehandle_, "palm_extras", 10));
163 
164 
165  // Debug real time publisher: publishes the raw ethercat data
168  return retval;
169 }
170 
177 void SR08::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
178 {
179  diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);
180 
181  stringstream name;
182  string prefix = device_id_.empty() ? device_id_ : (device_id_ + " ");
183  d.name = prefix + "EtherCAT Dual CAN Palm";
184  d.summary(d.OK, "OK");
185  stringstream hwid;
186  hwid << sh_->get_product_code() << "-" << sh_->get_serial();
187  d.hardware_id = hwid.str();
188 
189  d.clear();
190  d.addf("Position", "%02d", sh_->get_ring_position());
191  d.addf("Product Code", "%d", sh_->get_product_code());
192  d.addf("Serial Number", "%d", sh_->get_serial());
193  d.addf("Revision", "%d", sh_->get_revision());
194  d.addf("Counter", "%d", ++counter_);
195 
196  d.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
197  d.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
198  // reset the idle time min to a big number, to get a fresh number on next diagnostic
199  sr_hand_lib->main_pic_idle_time_min = 1000;
200 
201  this->ethercatDiagnostics(d, 2);
202  vec.push_back(d);
203 
204  // Add the diagnostics from the hand
205  sr_hand_lib->add_diagnostics(vec, d);
206 
207  // Add the diagnostics from the tactiles
208  if (sr_hand_lib->tactiles != NULL)
209  {
210  sr_hand_lib->tactiles->add_diagnostics(vec, d);
211  }
212 }
213 
229 void SR08::packCommand(unsigned char *buffer, bool halt, bool reset)
230 {
231  SrEdc::packCommand(buffer, halt, reset);
232 
233  ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND *command =
234  reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND *>(buffer);
235  ETHERCAT_CAN_BRIDGE_DATA *message = reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(buffer + ETHERCAT_COMMAND_DATA_SIZE);
236 
237  if (!flashing)
238  {
239  command->EDC_command = EDC_COMMAND_SENSOR_DATA;
240  }
241  else
242  {
243  command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
244  }
245 
246  // alternate between even and uneven motors
247  // and ask for the different informations.
248  sr_hand_lib->build_command(command);
249 
250  // @todo For the moment the aux_data_type in the commend will be fixed here. This is for convenience,
251  // before we separate the aux data (and the prox_mid data) from the UBIO sensor type in the driver.
252  // After that, this should be done in the aux_data_updater (or something similar ) in the driver
253  command->aux_data_type = TACTILE_SENSOR_TYPE_MCP320x_TACTILE;
254 
255  ROS_DEBUG(
256  "Sending command : Type : 0x%02X ; data : 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X"
257  " 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X",
258  command->to_motor_data_type,
259  command->motor_data[0],
260  command->motor_data[1],
261  command->motor_data[2],
262  command->motor_data[3],
263  command->motor_data[4],
264  command->motor_data[5],
265  command->motor_data[6],
266  command->motor_data[7],
267  command->motor_data[8],
268  command->motor_data[9],
269  command->motor_data[10],
270  command->motor_data[11],
271  command->motor_data[12],
272  command->motor_data[13],
273  command->motor_data[14],
274  command->motor_data[15],
275  command->motor_data[16],
276  command->motor_data[17],
277  command->motor_data[18],
278  command->motor_data[19]);
279 
280  build_CAN_message(message);
281 }
282 
301 bool SR08::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
302 {
303  ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS *status_data =
304  reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS *>(this_buffer + command_size_);
305  ETHERCAT_CAN_BRIDGE_DATA *can_data =
306  reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(this_buffer + command_size_ + ETHERCAT_STATUS_DATA_SIZE);
307  // int16u *status_buffer = (int16u*)status_data;
308  static unsigned int num_rxed_packets = 0;
309 
310  ++num_rxed_packets;
311 
312 
313  // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
314  if (debug_publisher->trylock())
315  {
316  debug_publisher->msg_.header.stamp = ros::Time::now();
317 
318  debug_publisher->msg_.sensors.clear();
319  for (unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
320  {
321  debug_publisher->msg_.sensors.push_back(status_data->sensors[i]);
322  }
323 
324  debug_publisher->msg_.motor_data_type.data = static_cast<int> (status_data->motor_data_type);
325  debug_publisher->msg_.which_motors = status_data->which_motors;
326  debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
327  debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;
328 
329  debug_publisher->msg_.motor_data_packet_torque.clear();
330  debug_publisher->msg_.motor_data_packet_misc.clear();
331  for (unsigned int i = 0; i < 10; ++i)
332  {
333  debug_publisher->msg_.motor_data_packet_torque.push_back(status_data->motor_data_packet[i].torque);
334  debug_publisher->msg_.motor_data_packet_misc.push_back(status_data->motor_data_packet[i].misc);
335  }
336 
337  debug_publisher->msg_.tactile_data_type = static_cast<unsigned int> (
338  static_cast<int32u>(status_data->tactile_data_type));
339  debug_publisher->msg_.tactile_data_valid = static_cast<unsigned int> (
340  static_cast<int16u> (status_data->tactile_data_valid));
341  debug_publisher->msg_.tactile.clear();
342  for (unsigned int i = 0; i < 5; ++i)
343  {
344  debug_publisher->msg_.tactile.push_back(
345  static_cast<unsigned int> (static_cast<int16u> (status_data->tactile[i].word[0])));
346  }
347 
348  debug_publisher->msg_.idle_time_us = status_data->idle_time_us;
349 
350  debug_publisher->unlockAndPublish();
351  }
352 
353  if (status_data->EDC_command == EDC_COMMAND_INVALID)
354  {
355  // received empty message: the pic is not writing to its mailbox.
357  float percentage_packet_loss = 100.f * (static_cast<float>(zero_buffer_read) /
358  static_cast<float>(num_rxed_packets));
359 
360  ROS_DEBUG("Reception error detected : %d errors out of %d rxed packets (%2.3f%%) ; idle time %dus",
361  zero_buffer_read, num_rxed_packets, percentage_packet_loss, status_data->idle_time_us);
362  return true;
363  }
364 
365  // We received a coherent message.
366  // Update the library (positions, diagnostics values, actuators, etc...)
367  // with the received information
368  sr_hand_lib->update(status_data);
369 
370  // Now publish the additional data at 100Hz (every 10 cycles)
371  if (cycle_count >= 10)
372  {
373  // publish tactiles if we have them
374  if (sr_hand_lib->tactiles != NULL)
375  {
376  sr_hand_lib->tactiles->publish();
377  }
378 
379  // And we also publish the additional data (accelerometer / gyroscope / analog inputs)
380  std_msgs::Float64MultiArray extra_analog_msg;
381  extra_analog_msg.layout.dim.resize(3);
382  extra_analog_msg.data.resize(3 + 3 + 4);
383  std::vector<double> data;
384 
385  extra_analog_msg.layout.dim[0].label = "accelerometer";
386  extra_analog_msg.layout.dim[0].size = 3;
387  extra_analog_msg.data[0] = status_data->sensors[ACCX];
388  extra_analog_msg.data[1] = status_data->sensors[ACCY];
389  extra_analog_msg.data[2] = status_data->sensors[ACCZ];
390 
391  extra_analog_msg.layout.dim[1].label = "gyrometer";
392  extra_analog_msg.layout.dim[1].size = 3;
393  extra_analog_msg.data[3] = status_data->sensors[GYRX];
394  extra_analog_msg.data[4] = status_data->sensors[GYRY];
395  extra_analog_msg.data[5] = status_data->sensors[GYRZ];
396 
397  extra_analog_msg.layout.dim[2].label = "analog_inputs";
398  extra_analog_msg.layout.dim[2].size = 4;
399  extra_analog_msg.data[6] = status_data->sensors[ANA0];
400  extra_analog_msg.data[7] = status_data->sensors[ANA1];
401  extra_analog_msg.data[8] = status_data->sensors[ANA2];
402  extra_analog_msg.data[9] = status_data->sensors[ANA3];
403 
404  if (extra_analog_inputs_publisher->trylock())
405  {
406  extra_analog_inputs_publisher->msg_ = extra_analog_msg;
407  extra_analog_inputs_publisher->unlockAndPublish();
408  }
409 
410  cycle_count = 0;
411  }
412  ++cycle_count;
413 
414 
415  // If we're flashing, check is the packet has been acked
416  if (flashing & !can_packet_acked)
417  {
418  if (can_data_is_ack(can_data))
419  {
420  can_packet_acked = true;
421  }
422  }
423 
424  return true;
425 }
426 
428 {
429  // Reinitialize motors information
430  sr_hand_lib->reinitialize_motors();
431 }
432 
433 void SR08::get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
434 {
435  // We're using 2 can busses,
436  // if motor id is between 0 and 9, then we're using the can_bus 1
437  // else, we're using the can bus 2.
438  int motor_id_tmp = board_id;
439  if (motor_id_tmp > 9)
440  {
441  motor_id_tmp -= 10;
442  *can_bus = 2;
443  }
444  else
445  {
446  *can_bus = 1;
447  }
448 
449  *board_can_id = motor_id_tmp;
450 }
451 
452 /* For the emacs weenies in the crowd.
453  Local Variables:
454  c-basic-offset: 2
455  End:
456 */
d
unsigned short int16u
ros::NodeHandle nh_tilde_
Definition: sr_edc.h:87
EDC_COMMAND_SENSOR_DATA
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)
Definition: sr0x.cpp:36
void summary(unsigned char lvl, const std::string s)
#define ETHERCAT_COMMAND_0230_AGREED_SIZE
ANA0
ACCZ
virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
Definition: sr08.cpp:433
boost::shared_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > > extra_analog_inputs_publisher
Extra analog inputs real time publisher (+ accelerometer and gyroscope)
Definition: sr08.h:76
This is a ROS driver for Shadow Robot #8 EtherCAT product ID.
SR08()
Constructor of the SR08 driver.
Definition: sr08.cpp:78
data
ACCX
virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address)
Construct function, run at startup to set SyncManagers and FMMUs.
Definition: sr08.cpp:125
#define ETHERCAT_STATUS_0230_AGREED_SIZE
boost::shared_ptr< shadow_robot::SrMotorHandLib< ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0230_PALM_EDC_COMMAND > > sr_hand_lib
Definition: sr08.h:98
#define ETHERCAT_STATUS_DATA_SIZE
Definition: sr08.cpp:59
ANA3
int counter_
Definition: sr_edc.h:85
void addf(const std::string &key, const char *format,...)
virtual void packCommand(unsigned char *buffer, bool halt, bool reset)
packs the commands before sending them to the EtherCAT bus
Definition: sr08.cpp:229
unsigned int int32u
BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND)==4)
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
#define ETHERCAT_COMMAND_DATA_ADDRESS
Definition: sr08.cpp:64
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)
Definition: sr08.cpp:141
bool can_packet_acked
Definition: sr_edc.h:96
ANA2
ROSLIB_DECL std::string command(const std::string &cmd)
Definition: sr08.h:55
#define ROS_INFO(...)
ros::NodeHandle nodehandle_
Definition: sr_edc.h:86
ACCY
virtual void reinitialize_boards()
This function will call the reinitialization function for the boards attached to the CAN bus...
Definition: sr08.cpp:427
void build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message)
Definition: sr_edc.cpp:615
#define ETHERCAT_COMMAND_DATA_SIZE
Definition: sr08.cpp:60
int16_t cycle_count
Definition: sr08.h:104
GYRY
EDC_COMMAND_INVALID
#define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
Definition: sr08.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
PLUGINLIB_EXPORT_CLASS(SR08, EthercatDevice)
#define ETHERCAT_CAN_BRIDGE_DATA_SIZE
Definition: sr08.cpp:62
GYRZ
static Time now()
ANA1
TACTILE_SENSOR_TYPE_MCP320x_TACTILE
std::string device_joint_prefix_
Definition: sr_edc.h:99
std::string device_id_
Definition: sr_edc.h:98
GYRX
#define ROS_ASSERT(cond)
EDC_COMMAND
virtual void multiDiagnostics(vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
This function gives some diagnostics data.
Definition: sr08.cpp:177
virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
This functions receives data from the EtherCAT bus.
Definition: sr08.cpp:301
#define SENSORS_NUM_0220
bool flashing
Definition: sr_edc.h:95
#define ETHERCAT_STATUS_DATA_ADDRESS
Definition: sr08.cpp:65
#define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
Definition: sr08.cpp:67
#define ROS_DEBUG(...)
unsigned int zero_buffer_read
Definition: sr08.h:95
boost::shared_ptr< realtime_tools::RealtimePublisher< sr_robot_msgs::EthercatDebug > > debug_publisher
Debug real time publisher: publishes the raw ethercat data.
Definition: sr08.h:107


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