sr_edc_muscle.cpp
Go to the documentation of this file.
1 
27 
29 
30 #include <sstream>
31 #include <string>
32 #include <vector>
33 #include <iomanip>
34 #include <boost/foreach.hpp>
35 #include <std_msgs/Int16.h>
36 #include <math.h>
37 #include <fcntl.h>
38 #include <stdio.h>
39 #include <pthread.h>
40 
41 #include <sr_utilities/sr_math_utils.hpp>
42 
43 using std::string;
44 using std::stringstream;
45 using std::vector;
46 
48 
49 #include <boost/static_assert.hpp>
50 
51 namespace is_edc_command_32_bits
52 {
53 // check is the EDC_COMMAND is 32bits on the computer
54 // if not, fails
55  BOOST_STATIC_ASSERT(sizeof(EDC_COMMAND) == 4);
56 } // namespace is_edc_command_32_bits
57 
58 #define ETHERCAT_STATUS_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS)
59 #define ETHERCAT_COMMAND_DATA_SIZE sizeof(ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND)
60 
61 #define ETHERCAT_CAN_BRIDGE_DATA_SIZE sizeof(ETHERCAT_CAN_BRIDGE_DATA)
62 
63 #define ETHERCAT_COMMAND_DATA_ADDRESS PALM_0300_ETHERCAT_COMMAND_DATA_ADDRESS
64 #define ETHERCAT_STATUS_DATA_ADDRESS PALM_0300_ETHERCAT_STATUS_DATA_ADDRESS
65 #define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS PALM_0300_ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
66 #define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS PALM_0300_ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
67 
68 
69 PLUGINLIB_EXPORT_CLASS(SrEdcMuscle, EthercatDevice);
70 
78  : zero_buffer_read(0),
79  cycle_count(0)
80 {
81  /*
82  ROS_INFO("There are %d sensors", nb_sensors_const);
83  ROS_INFO( "device_pub_freq_const = %d", device_pub_freq_const );
84  ROS_INFO( "ros_pub_freq_const = %d", ros_pub_freq_const );
85  ROS_INFO( "max_iter_const = %d", max_iter_const );
86  ROS_INFO( "nb_sensors_const = %d", nb_sensors_const );
87  ROS_INFO("nb_publish_by_unpack_const = %d", nb_publish_by_unpack_const );
88  */
89 }
90 
124 void SrEdcMuscle::construct(EtherCAT_SlaveHandler *sh, int &start_address)
125 {
128 
133 
134  ROS_INFO("Finished constructing the SrEdcMuscle driver");
135 }
136 
140 int SrEdcMuscle::initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
141 {
142  int retval = SR0X::initialize(hw, allow_unprogrammed);
143 
144  if (retval != 0)
145  {
146  return retval;
147  }
148 
149  sr_hand_lib = boost::shared_ptr<shadow_robot::SrMuscleHandLib<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS,
150  ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND> >(
151  new shadow_robot::SrMuscleHandLib<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS,
152  ETHERCAT_DATA_STRUCTURE_0300_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  return retval;
168 }
169 
176 void SrEdcMuscle::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
177 {
178  diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);
179 
180  stringstream name;
181  string prefix = device_id_.empty() ? device_id_ : (device_id_ + " ");
182  d.name = prefix + "EtherCAT Dual CAN Palm";
183  d.summary(d.OK, "OK");
184  stringstream hwid;
185  hwid << sh_->get_product_code() << "-" << sh_->get_serial();
186  d.hardware_id = hwid.str();
187 
188  d.clear();
189  d.addf("Position", "%02d", sh_->get_ring_position());
190  d.addf("Product Code", "%d", sh_->get_product_code());
191  d.addf("Serial Number", "%d", sh_->get_serial());
192  d.addf("Revision", "%d", sh_->get_revision());
193  d.addf("Counter", "%d", ++counter_);
194 
195  d.addf("PIC idle time (in microsecs)", "%d", sr_hand_lib->main_pic_idle_time);
196  d.addf("Min PIC idle time (since last diagnostics)", "%d", sr_hand_lib->main_pic_idle_time_min);
197  // reset the idle time min to a big number, to get a fresh number on next diagnostic
198  sr_hand_lib->main_pic_idle_time_min = 1000;
199 
200  this->ethercatDiagnostics(d, 2);
201  vec.push_back(d);
202 
203  // Add the diagnostics from the hand
204  sr_hand_lib->add_diagnostics(vec, d);
205 
206  // Add the diagnostics from the tactiles
207  if (sr_hand_lib->tactiles != NULL)
208  {
209  sr_hand_lib->tactiles->add_diagnostics(vec, d);
210  }
211 }
212 
228 void SrEdcMuscle::packCommand(unsigned char *buffer, bool halt, bool reset)
229 {
230  SrEdc::packCommand(buffer, halt, reset);
231 
232  ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND *command =
233  reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND *>(buffer);
234  ETHERCAT_CAN_BRIDGE_DATA *message = reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(buffer + ETHERCAT_COMMAND_DATA_SIZE);
235 
236  if (!flashing)
237  {
238  command->EDC_command = EDC_COMMAND_SENSOR_DATA;
239  }
240  else
241  {
242  command->EDC_command = EDC_COMMAND_CAN_DIRECT_MODE;
243  }
244 
245  // alternate between even and uneven motors
246  // and ask for the different informations.
247  sr_hand_lib->build_command(command);
248 
249  ROS_DEBUG(
250  "Sending command : Type : 0x%02X ; data : 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X "
251  "0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X 0x%04X",
252  command->to_muscle_data_type,
253  command->muscle_data[0],
254  command->muscle_data[1],
255  command->muscle_data[2],
256  command->muscle_data[3],
257  command->muscle_data[4],
258  command->muscle_data[5],
259  command->muscle_data[6],
260  command->muscle_data[7],
261  command->muscle_data[8],
262  command->muscle_data[9],
263  command->muscle_data[10],
264  command->muscle_data[11],
265  command->muscle_data[12],
266  command->muscle_data[13],
267  command->muscle_data[14],
268  command->muscle_data[15],
269  command->muscle_data[16],
270  command->muscle_data[17],
271  command->muscle_data[18],
272  command->muscle_data[19]);
273 
274  build_CAN_message(message);
275 }
276 
295 bool SrEdcMuscle::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
296 {
297  ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS *status_data =
298  reinterpret_cast<ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS *>(this_buffer + command_size_);
299  ETHERCAT_CAN_BRIDGE_DATA *can_data =
300  reinterpret_cast<ETHERCAT_CAN_BRIDGE_DATA *>(this_buffer + command_size_ + ETHERCAT_STATUS_DATA_SIZE);
301  // int16u *status_buffer = (int16u*)status_data;
302  static unsigned int num_rxed_packets = 0;
303 
304  ++num_rxed_packets;
305 
306 
307  // publishes the debug information (a slightly formatted version of the incoming ethercat packet):
308  if (debug_publisher->trylock())
309  {
310  debug_publisher->msg_.header.stamp = ros::Time::now();
311 
312  debug_publisher->msg_.sensors.clear();
313  for (unsigned int i = 0; i < SENSORS_NUM_0220 + 1; ++i)
314  {
315  debug_publisher->msg_.sensors.push_back(status_data->sensors[i]);
316  }
317  /*
318  debug_publisher->msg_.motor_data_type.data = static_cast<int>(status_data->motor_data_type);
319  debug_publisher->msg_.which_motors = status_data->which_motors;
320  debug_publisher->msg_.which_motor_data_arrived = status_data->which_motor_data_arrived;
321  debug_publisher->msg_.which_motor_data_had_errors = status_data->which_motor_data_had_errors;
322  */
323  debug_publisher->msg_.motor_data_packet_torque.clear();
324  debug_publisher->msg_.motor_data_packet_misc.clear();
325 
326  for (unsigned int i = 0; i < 8; ++i)
327  {
328  debug_publisher->msg_.motor_data_packet_torque.push_back(
329  (status_data->muscle_data_packet[i].packed.pressure0_H << 8) +
330  (status_data->muscle_data_packet[i].packed.pressure0_M << 4) +
331  status_data->muscle_data_packet[i].packed.pressure0_L);
332  debug_publisher->msg_.motor_data_packet_torque.push_back(
333  (status_data->muscle_data_packet[i].packed.pressure1_H << 8) +
334  (status_data->muscle_data_packet[i].packed.pressure1_M << 4) +
335  status_data->muscle_data_packet[i].packed.pressure1_L);
336  debug_publisher->msg_.motor_data_packet_torque.push_back(
337  (status_data->muscle_data_packet[i].packed.pressure2_H << 8) +
338  (status_data->muscle_data_packet[i].packed.pressure2_M << 4) +
339  status_data->muscle_data_packet[i].packed.pressure2_L);
340  debug_publisher->msg_.motor_data_packet_torque.push_back(
341  (status_data->muscle_data_packet[i].packed.pressure3_H << 8) +
342  (status_data->muscle_data_packet[i].packed.pressure3_M << 4) +
343  status_data->muscle_data_packet[i].packed.pressure3_L);
344  debug_publisher->msg_.motor_data_packet_torque.push_back(
345  (status_data->muscle_data_packet[i].packed.pressure4_H << 8) +
346  (status_data->muscle_data_packet[i].packed.pressure4_M << 4) +
347  status_data->muscle_data_packet[i].packed.pressure4_L);
348  }
349  /*
350  for(unsigned int i=0; i < 10; ++i)
351  {
352  debug_publisher->msg_.motor_data_packet_torque.push_back( status_data->motor_data_packet[i].torque );
353  debug_publisher->msg_.motor_data_packet_misc.push_back( status_data->motor_data_packet[i].misc );
354  }
355  */
356  debug_publisher->msg_.tactile_data_type =
357  static_cast<unsigned int> (static_cast<int32u> (status_data->tactile_data_type));
358  debug_publisher->msg_.tactile_data_valid =
359  static_cast<unsigned int> (static_cast<int16u> (status_data->tactile_data_valid));
360  debug_publisher->msg_.tactile.clear();
361  for (unsigned int i = 0; i < 5; ++i)
362  {
363  debug_publisher->msg_.tactile.push_back(
364  static_cast<unsigned int> (static_cast<int16u> (status_data->tactile[i].word[0])));
365  }
366 
367  debug_publisher->msg_.idle_time_us = status_data->idle_time_us;
368 
369  debug_publisher->unlockAndPublish();
370  }
371 
372  if (status_data->EDC_command == EDC_COMMAND_INVALID)
373  {
374  // received empty message: the pic is not writing to its mailbox.
376  float percentage_packet_loss = 100.f * (static_cast<float>(zero_buffer_read)/ static_cast<float>(num_rxed_packets));
377 
378  ROS_DEBUG("Reception error detected : %d errors out of %d rxed packets (%2.3f%%) ; idle time %dus",
379  zero_buffer_read, num_rxed_packets, percentage_packet_loss, status_data->idle_time_us);
380  return true;
381  }
382 
383  // We received a coherent message.
384  // Update the library (positions, diagnostics values, actuators, etc...)
385  // with the received information
386  sr_hand_lib->update(status_data);
387 
388  // Now publish the additional data at 100Hz (every 10 cycles)
389  if (cycle_count >= 9)
390  {
391  // publish tactiles if we have them
392  if (sr_hand_lib->tactiles != NULL)
393  {
394  sr_hand_lib->tactiles->publish();
395  }
396 
397  // And we also publish the additional data (accelerometer / gyroscope / analog inputs)
398  std_msgs::Float64MultiArray extra_analog_msg;
399  extra_analog_msg.layout.dim.resize(3);
400  extra_analog_msg.data.resize(3 + 3 + 4);
401  std::vector<double> data;
402 
403  extra_analog_msg.layout.dim[0].label = "accelerometer";
404  extra_analog_msg.layout.dim[0].size = 3;
405  extra_analog_msg.data[0] = status_data->sensors[ACCX];
406  extra_analog_msg.data[1] = status_data->sensors[ACCY];
407  extra_analog_msg.data[2] = status_data->sensors[ACCZ];
408 
409  extra_analog_msg.layout.dim[1].label = "gyrometer";
410  extra_analog_msg.layout.dim[1].size = 3;
411  extra_analog_msg.data[3] = status_data->sensors[GYRX];
412  extra_analog_msg.data[4] = status_data->sensors[GYRY];
413  extra_analog_msg.data[5] = status_data->sensors[GYRZ];
414 
415  extra_analog_msg.layout.dim[2].label = "analog_inputs";
416  extra_analog_msg.layout.dim[2].size = 4;
417  extra_analog_msg.data[6] = status_data->sensors[ANA0];
418  extra_analog_msg.data[7] = status_data->sensors[ANA1];
419  extra_analog_msg.data[8] = status_data->sensors[ANA2];
420  extra_analog_msg.data[9] = status_data->sensors[ANA3];
421 
422  if (extra_analog_inputs_publisher->trylock())
423  {
424  extra_analog_inputs_publisher->msg_ = extra_analog_msg;
425  extra_analog_inputs_publisher->unlockAndPublish();
426  }
427 
428  cycle_count = 0;
429  }
430  ++cycle_count;
431 
432 
433  // If we're flashing, check is the packet has been acked
434  if (flashing & !can_packet_acked)
435  {
436  if (can_data_is_ack(can_data))
437  {
438  can_packet_acked = true;
439  }
440  }
441 
442  return true;
443 }
444 
446 {
447  // Reinitialize motors information
448  sr_hand_lib->reinitialize_motors();
449 }
450 
451 void SrEdcMuscle::get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
452 {
453  // We're using 2 can busses,
454  // if muscle_driver_board id is between 0 and 1, then we're using the can_bus 1
455  // else, we're using the can bus 2.
456  int muscle_driver_board_id_tmp = board_id;
457  if (muscle_driver_board_id_tmp > 1)
458  {
459  muscle_driver_board_id_tmp -= 2;
460  *can_bus = 2;
461  }
462  else
463  {
464  *can_bus = 1;
465  }
466 
467  *board_can_id = muscle_driver_board_id_tmp;
468 }
469 
470 /* For the emacs weenies in the crowd.
471  Local Variables:
472  c-basic-offset: 2
473  End:
474 */
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)
ANA0
ACCZ
data
ACCX
#define ETHERCAT_CAN_BRIDGE_DATA_COMMAND_ADDRESS
ANA3
int counter_
Definition: sr_edc.h:85
void addf(const std::string &key, const char *format,...)
boost::shared_ptr< realtime_tools::RealtimePublisher< sr_robot_msgs::EthercatDebug > > debug_publisher
Debug real time publisher: publishes the raw ethercat data.
unsigned int int32u
#define ETHERCAT_COMMAND_0300_AGREED_SIZE
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
bool can_packet_acked
Definition: sr_edc.h:96
ANA2
virtual void reinitialize_boards()
This function will call the reinitialization function for the boards attached to the CAN bus...
#define ETHERCAT_COMMAND_DATA_SIZE
ROSLIB_DECL std::string command(const std::string &cmd)
#define ETHERCAT_CAN_BRIDGE_DATA_SIZE
#define ETHERCAT_STATUS_0300_AGREED_SIZE
#define ETHERCAT_STATUS_DATA_SIZE
#define ROS_INFO(...)
ros::NodeHandle nodehandle_
Definition: sr_edc.h:86
#define ETHERCAT_CAN_BRIDGE_DATA_STATUS_ADDRESS
ACCY
void build_CAN_message(ETHERCAT_CAN_BRIDGE_DATA *message)
Definition: sr_edc.cpp:615
virtual void get_board_id_and_can_bus(int board_id, int *can_bus, unsigned int *board_can_id)
GYRY
EDC_COMMAND_INVALID
This is a ROS driver for Shadow Robot EtherCAT dual CAN muscle hand.
virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address)
Construct function, run at startup to set SyncManagers and FMMUs.
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
#define ETHERCAT_COMMAND_DATA_ADDRESS
#define ETHERCAT_STATUS_DATA_ADDRESS
virtual void multiDiagnostics(vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
This function gives some diagnostics data.
GYRZ
static Time now()
ANA1
int16_t cycle_count
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 bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
This functions receives data from the EtherCAT bus.
#define SENSORS_NUM_0220
virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed=true)
PLUGINLIB_EXPORT_CLASS(SrEdcMuscle, EthercatDevice)
unsigned int zero_buffer_read
Definition: sr_edc_muscle.h:94
bool flashing
Definition: sr_edc.h:95
boost::shared_ptr< shadow_robot::SrMuscleHandLib< ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_STATUS, ETHERCAT_DATA_STRUCTURE_0300_PALM_EDC_COMMAND > > sr_hand_lib
Definition: sr_edc_muscle.h:97
boost::shared_ptr< realtime_tools::RealtimePublisher< std_msgs::Float64MultiArray > > extra_analog_inputs_publisher
Extra analog inputs real time publisher (+ accelerometer and gyroscope)
Definition: sr_edc_muscle.h:75
virtual void packCommand(unsigned char *buffer, bool halt, bool reset)
packs the commands before sending them to the EtherCAT bus
#define ROS_DEBUG(...)
SrEdcMuscle()
Constructor of the SrEdcMuscle driver.


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