wg06.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include <iomanip>
36 
37 #include <algorithm>
38 
39 #include <math.h>
40 #include <stddef.h>
41 
42 #include <ethercat_hardware/wg06.h>
43 
44 #include <dll/ethercat_dll.h>
45 #include <al/ethercat_AL.h>
46 #include <dll/ethercat_device_addressed_telegram.h>
47 #include <dll/ethercat_frame.h>
48 
49 #include <boost/static_assert.hpp>
50 
52 
54 
55 
57  has_accel_and_ft_(false),
58  pressure_checksum_error_(false),
59  pressure_checksum_error_count_(0),
60  accelerometer_samples_(0),
61  accelerometer_missed_samples_(0),
62  first_publish_(true),
63  last_pressure_time_(0),
64  pressure_publisher_(NULL),
65  accel_publisher_(NULL),
66  ft_overload_limit_(31100),
67  ft_overload_flags_(0),
68  ft_disconnected_(false),
69  ft_vhalf_error_(false),
70  ft_sampling_rate_error_(false),
72  ft_missed_samples_(0),
73  diag_last_ft_sample_count_(0),
74  raw_ft_publisher_(NULL),
75  ft_publisher_(NULL),
76  enable_pressure_sensor_(true),
77  enable_ft_sensor_(false),
78  enable_soft_processor_access_(true)
79  // ft_publisher_(NULL)
80 {
81 
82 }
83 
85 {
88 }
89 
90 void WG06::construct(EtherCAT_SlaveHandler *sh, int &start_address)
91 {
92  WG0X::construct(sh, start_address);
93 
94  has_accel_and_ft_ = false;
95 
96  // As good a place as any for making sure that compiler actually packed these structures correctly
97  BOOST_STATIC_ASSERT(sizeof(WG06StatusWithAccel) == WG06StatusWithAccel::SIZE);
98  BOOST_STATIC_ASSERT(sizeof(FTDataSample) == FTDataSample::SIZE);
99  BOOST_STATIC_ASSERT(sizeof(WG06Pressure) == WG06Pressure::SIZE);
100  BOOST_STATIC_ASSERT(sizeof(WG06BigPressure) == WG06BigPressure::SIZE);
101  BOOST_STATIC_ASSERT(sizeof(WG06StatusWithAccelAndFT) == WG06StatusWithAccelAndFT::SIZE);
102 
103  unsigned int base_status_size = sizeof(WG0XStatus);
104 
105  command_size_ = sizeof(WG0XCommand);
106  status_size_ = sizeof(WG0XStatus);
107  pressure_size_ = sizeof(WG06Pressure);
108  unsigned pressure_phy_addr = PRESSURE_PHY_ADDR;
109 
110  if (fw_major_ == 0)
111  {
112  // Do nothing - status memory map is same size as WG05
113  }
114  if (fw_major_ == 1)
115  {
116  // Include Accelerometer data
117  status_size_ = base_status_size = sizeof(WG06StatusWithAccel);
118  }
119  else if ((fw_major_ == 2) || (fw_major_ == 3))
120  {
121  // Include Accelerometer and Force/Torque sensor data
122  status_size_ = base_status_size = sizeof(WG06StatusWithAccelAndFT);
123  has_accel_and_ft_ = true;
124  if (fw_major_ == 3)
125  {
126  // Pressure data size is 513 bytes instead of 94. The physical address has also moved.
128  pressure_phy_addr = BIG_PRESSURE_PHY_ADDR;
129  }
130  }
131  else
132  {
133  ROS_ERROR("Unsupported WG06 FW major version %d", fw_major_);
134  }
136 
137 
138  EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(3);
139  //ROS_DEBUG("device %d, command 0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
140  (*fmmu)[0] = EC_FMMU(start_address, // Logical start address
141  command_size_,// Logical length
142  0x00, // Logical StartBit
143  0x07, // Logical EndBit
144  COMMAND_PHY_ADDR, // Physical Start address
145  0x00, // Physical StartBit
146  false, // Read Enable
147  true, // Write Enable
148  true); // Enable
149 
150  start_address += command_size_;
151 
152  //ROS_DEBUG("device %d, status 0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
153  (*fmmu)[1] = EC_FMMU(start_address, // Logical start address
154  base_status_size, // Logical length
155  0x00, // Logical StartBit
156  0x07, // Logical EndBit
157  STATUS_PHY_ADDR, // Physical Start address
158  0x00, // Physical StartBit
159  true, // Read Enable
160  false, // Write Enable
161  true); // Enable
162 
163  start_address += base_status_size;
164 
165  (*fmmu)[2] = EC_FMMU(start_address, // Logical start address
166  pressure_size_, // Logical length
167  0x00, // Logical StartBit
168  0x07, // Logical EndBit
169  pressure_phy_addr, // Physical Start address
170  0x00, // Physical StartBit
171  true, // Read Enable
172  false, // Write Enable
173  true); // Enable
174 
175  start_address += pressure_size_;
176 
177  sh->set_fmmu_config(fmmu);
178 
179  EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(5);
180 
181  // Sync managers
182  (*pd)[0] = EC_SyncMan(COMMAND_PHY_ADDR, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
183  (*pd)[0].ChannelEnable = true;
184  (*pd)[0].ALEventEnable = true;
185 
186  (*pd)[1] = EC_SyncMan(STATUS_PHY_ADDR, base_status_size);
187  (*pd)[1].ChannelEnable = true;
188 
189  (*pd)[2] = EC_SyncMan(WGMailbox::MBX_COMMAND_PHY_ADDR, WGMailbox::MBX_COMMAND_SIZE, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
190  (*pd)[2].ChannelEnable = true;
191  (*pd)[2].ALEventEnable = true;
192 
193  (*pd)[3] = EC_SyncMan(WGMailbox::MBX_STATUS_PHY_ADDR, WGMailbox::MBX_STATUS_SIZE, EC_QUEUED);
194  (*pd)[3].ChannelEnable = true;
195 
196  (*pd)[4] = EC_SyncMan(pressure_phy_addr, pressure_size_);
197  (*pd)[4].ChannelEnable = true;
198 
199  sh->set_pd_config(pd);
200 }
201 
202 
204 {
205  if ( ((fw_major_ == 1) && (fw_minor_ >= 1)) || (fw_major_ >= 2) )
206  {
208  }
209 
210  int retval = WG0X::initialize(hw, allow_unprogrammed);
211 
212  if (!retval && use_ros_)
213  {
214  bool poor_measured_motor_voltage = false;
215  double max_pwm_ratio = double(0x2700) / double(PWM_MAX);
216  double board_resistance = 5.0;
217  if (!WG0X::initializeMotorModel(hw, "WG006", max_pwm_ratio, board_resistance, poor_measured_motor_voltage))
218  {
219  ROS_FATAL("Initializing motor trace failed");
220  sleep(1); // wait for ros to flush rosconsole output
221  return -1;
222  }
223 
224  // For some versions of software pressure and force/torque sensors can be
225  // selectively enabled / disabled
226  ros::NodeHandle nh(string("~/") + actuator_.name_);
227  if (!nh.getParam("enable_pressure_sensor", enable_pressure_sensor_))
228  {
229  enable_pressure_sensor_ = true; //default to to true
230  }
231  if (!nh.getParam("enable_ft_sensor", enable_ft_sensor_))
232  {
233  enable_ft_sensor_ = false; //default to to false
234  }
235 
236  if (enable_ft_sensor_ && (fw_major_ < 2))
237  {
238  ROS_WARN("Gripper firmware version %d does not support enabling force/torque sensor", fw_major_);
239  enable_ft_sensor_ = false;
240  }
241 
242  // FW version 2+ supports selectively enabling/disabling pressure and F/T sensor
243  if (fw_major_ >= 2)
244  {
245  static const uint8_t PRESSURE_ENABLE_FLAG = 0x1;
246  static const uint8_t FT_ENABLE_FLAG = 0x2;
247  static const unsigned PRESSURE_FT_ENABLE_ADDR = 0xAA;
248  uint8_t pressure_ft_enable = 0;
249  if (enable_pressure_sensor_) pressure_ft_enable |= PRESSURE_ENABLE_FLAG;
250  if (enable_ft_sensor_) pressure_ft_enable |= FT_ENABLE_FLAG;
251  EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
252  if (writeMailbox(&com, PRESSURE_FT_ENABLE_ADDR, &pressure_ft_enable, 1) != 0)
253  {
254  ROS_FATAL("Could not enable/disable pressure and force/torque sensors");
255  return -1;
256  }
257  }
258 
259  if (!initializePressure(hw))
260  {
261  return -1;
262  }
263 
264  // Publish accelerometer data as a ROS topic, if firmware is recent enough
265  if (fw_major_ >= 1)
266  {
267  if (!initializeAccel(hw))
268  {
269  return -1;
270  }
271  }
272 
273  // FW version 2 supports Force/Torque sensor.
274  // Provide Force/Torque data to controllers as an AnalogIn vector
275  if ((fw_major_ >= 2) && (enable_ft_sensor_))
276  {
277  if (!initializeFT(hw))
278  {
279  return -1;
280  }
281  }
282 
283  // FW version 2 and 3 uses soft-processors to control certain peripherals.
284  // Allow the firmware on these soft-processors to be read/write through ROS service calls
286  {
288  {
289  return -1;
290  }
291  }
292 
293 
294  }
295 
296  return retval;
297 }
298 
299 
301 {
302  // Publish pressure sensor data as a ROS topic
303  string topic = "pressure";
304  if (!actuator_.name_.empty())
305  topic = topic + "/" + string(actuator_.name_);
307 
308  // Register pressure sensor with pr2_hardware_interface::HardwareInterface
309  for (int i = 0; i < 2; ++i)
310  {
311  pressure_sensors_[i].state_.data_.resize(22);
312  pressure_sensors_[i].name_ = string(actuator_info_.name_) + string(i ? "r_finger_tip" : "l_finger_tip");
313  if (hw && !hw->addPressureSensor(&pressure_sensors_[i]))
314  {
315  ROS_FATAL("A pressure sensor of the name '%s' already exists. Device #%02d has a duplicate name", pressure_sensors_[i].name_.c_str(), sh_->get_ring_position());
316  return false;
317  }
318  }
319 
320  return true;
321 }
322 
323 
325 {
326  string topic = "accelerometer";
327  if (!actuator_.name_.empty())
328  {
329  topic = topic + "/" + string(actuator_.name_);
330  }
332 
333  // Register accelerometer with pr2_hardware_interface::HardwareInterface
335  if (hw && !hw->addAccelerometer(&accelerometer_))
336  {
337  ROS_FATAL("An accelerometer of the name '%s' already exists. Device #%02d has a duplicate name", accelerometer_.name_.c_str(), sh_->get_ring_position());
338  return false;
339  }
340  return true;
341 }
342 
343 
345 {
346  ft_raw_analog_in_.name_ = actuator_.name_ + "_ft_raw";
347  if (hw && !hw->addAnalogIn(&ft_raw_analog_in_))
348  {
349  ROS_FATAL("An analog in of the name '%s' already exists. Device #%02d has a duplicate name",
350  ft_raw_analog_in_.name_.c_str(), sh_->get_ring_position());
351  return false;
352  }
353 
354  // FT provides 6 values : 3 Forces + 3 Torques
355  ft_raw_analog_in_.state_.state_.resize(6);
356  // FT usually provides 3-4 new samples per cycle
357  force_torque_.state_.samples_.reserve(4);
358  force_torque_.state_.good_ = true;
359 
360  // For now publish RAW F/T values for engineering purposes. In future this publisher may be disabled by default.
361  std::string topic = "raw_ft";
362  if (!actuator_.name_.empty())
363  topic = topic + "/" + string(actuator_.name_);
365  if (raw_ft_publisher_ == NULL)
366  {
367  ROS_FATAL("Could not allocate raw_ft publisher");
368  return false;
369  }
370  // Allocate space for raw f/t data values
371  raw_ft_publisher_->msg_.samples.reserve(MAX_FT_SAMPLES);
372 
374  force_torque_.state_.good_ = true;
375 
376  if (!actuator_.name_.empty())
377  {
378  ft_analog_in_.state_.state_.resize(6);
379  ros::NodeHandle nh("~" + string(actuator_.name_));
380  FTParamsInternal ft_params;
381  if ( ft_params.getRosParams(nh) )
382  {
383  ft_params_ = ft_params;
384  ft_params_.print();
385  // If we have ft_params, publish F/T values.
386  topic = "ft";
387  if (!actuator_.name_.empty())
388  topic = topic + "/" + string(actuator_.name_);
390  if (ft_publisher_ == NULL)
391  {
392  ROS_FATAL("Could not allocate ft publisher");
393  return false;
394  }
395 
396  // Register force/torque sensor with pr2_hardware_interface::HardwareInterface
398  if (hw && !hw->addForceTorque(&force_torque_))
399  {
400  ROS_FATAL("A force/torque sensor of the name '%s' already exists. Device #%02d has a duplicate name", force_torque_.name_.c_str(), sh_->get_ring_position());
401  return false;
402  }
403  }
404  }
405 
406  return true;
407 }
408 
409 
411 {
412  // TODO: do not use direct access to device. Not safe while realtime loop is running
413  // ALSO, this leaks memory
414  EthercatDirectCom *com = new EthercatDirectCom(EtherCAT_DataLinkLayer::instance());
415 
416  // Add soft-processors to list
417  soft_processor_.add(&mailbox_, actuator_.name_, "pressure", 0xA000, 0x249);
418  soft_processor_.add(&mailbox_, actuator_.name_, "accel", 0xB000, 0x24A);
419 
420  // Start services
421  if (!soft_processor_.initialize(com))
422  {
423  return false;
424  }
425 
426  return true;
427 }
428 
429 
430 
431 void WG06::packCommand(unsigned char *buffer, bool halt, bool reset)
432 {
433  if (reset)
434  {
435  pressure_checksum_error_ = false;
436  ft_overload_flags_ = 0;
437  ft_disconnected_ = false;
438  ft_vhalf_error_ = false;
439  ft_sampling_rate_error_ = false;
440  }
441 
442  WG0X::packCommand(buffer, halt, reset);
443 
444  WG0XCommand *c = (WG0XCommand *)buffer;
445 
446  if (accelerometer_.command_.range_ > 2 ||
449 
453 
455  ((accelerometer_.command_.bandwidth_ & 0x7) << 1) |
456  ((accelerometer_.command_.range_ & 0x3) << 4);
458 }
459 
460 
461 bool WG06::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
462 {
463  bool rv = true;
464 
465  int status_bytes =
466  has_accel_and_ft_ ? sizeof(WG06StatusWithAccelAndFT) : // Has FT sensor and accelerometer
468  sizeof(WG0XStatus);
469 
470  unsigned char *pressure_buf = (this_buffer + command_size_ + status_bytes);
471 
472  unsigned char* this_status = this_buffer + command_size_;
473  if (!verifyChecksum(this_status, status_bytes))
474  {
475  status_checksum_error_ = true;
476  rv = false;
477  goto end;
478  }
479 
480  if (!unpackPressure(pressure_buf))
481  {
482  rv = false;
483  //goto end; // all other tasks should not be effected by bad pressure sensor data
484  }
485 
486  if (accel_publisher_)
487  {
489  WG06StatusWithAccel *last_status = (WG06StatusWithAccel *)(prev_buffer + command_size_);
490  if (!unpackAccel(status, last_status))
491  {
492  rv=false;
493  }
494  }
495 
497  {
499  WG06StatusWithAccelAndFT *last_status = (WG06StatusWithAccelAndFT *)(prev_buffer + command_size_);
500  if (!unpackFT(status, last_status))
501  {
502  rv = false;
503  }
504  }
505 
506 
507  if (!WG0X::unpackState(this_buffer, prev_buffer))
508  {
509  rv = false;
510  }
511 
512  end:
513  return rv;
514 }
515 
516 
522 bool WG06::unpackPressure(unsigned char *pressure_buf)
523 {
525  {
526  // If pressure sensor is not enabled don't attempt to do anything with pressure data
527  return true;
528  }
529 
530  if (!verifyChecksum(pressure_buf, pressure_size_))
531  {
533  if (false /* debugging */)
534  {
535  std::stringstream ss;
536  ss << "Pressure buffer checksum error : " << std::endl;
537  for (unsigned ii=0; ii<pressure_size_; ++ii)
538  {
539  ss << std::uppercase << std::hex << std::setw(2) << std::setfill('0')
540  << unsigned(pressure_buf[ii]) << " ";
541  if ((ii%8) == 7) ss << std::endl;
542  }
543  ROS_ERROR_STREAM(ss.str());
544  std::cerr << ss.str() << std::endl;
545  }
547  return false;
548  }
549  else
550  {
551  WG06Pressure *p( (WG06Pressure *) pressure_buf);
552  for (int i = 0; i < 22; ++i ) {
554  ((p->l_finger_tip_[i] >> 8) & 0xff) |
555  ((p->l_finger_tip_[i] << 8) & 0xff00);
557  ((p->r_finger_tip_[i] >> 8) & 0xff) |
558  ((p->r_finger_tip_[i] << 8) & 0xff00);
559  }
560 
562  {
564  {
565  pressure_publisher_->msg_.header.stamp = ros::Time::now();
566  pressure_publisher_->msg_.l_finger_tip.resize(22);
567  pressure_publisher_->msg_.r_finger_tip.resize(22);
568  for (int i = 0; i < 22; ++i ) {
569  pressure_publisher_->msg_.l_finger_tip[i] = pressure_sensors_[0].state_.data_[i];
570  pressure_publisher_->msg_.r_finger_tip[i] = pressure_sensors_[1].state_.data_[i];
571  }
573  }
574  }
576  }
577 
578  return true;
579 }
580 
581 
588 {
589  int count = uint8_t(status->accel_count_ - last_status->accel_count_);
590  accelerometer_samples_ += count;
591  // Only most recent 4 samples of accelerometer data is available in status data
592  // 4 samples will be enough with realtime loop running at 1kHz and accelerometer running at 3kHz
593  // If count is greater than 4, then some data has been "missed".
594  accelerometer_missed_samples_ += (count > 4) ? (count-4) : 0;
595  count = min(4, count);
596  accelerometer_.state_.samples_.resize(count);
597  accelerometer_.state_.frame_id_ = string(actuator_info_.name_) + "_accelerometer_link";
598  for (int i = 0; i < count; ++i)
599  {
600  int32_t acc = status->accel_[count - i - 1];
601  int range = (acc >> 30) & 3;
602  float d = 1 << (8 - range);
603  accelerometer_.state_.samples_[i].x = 9.81 * ((((acc >> 0) & 0x3ff) << 22) >> 22) / d;
604  accelerometer_.state_.samples_[i].y = 9.81 * ((((acc >> 10) & 0x3ff) << 22) >> 22) / d;
605  accelerometer_.state_.samples_[i].z = 9.81 * ((((acc >> 20) & 0x3ff) << 22) >> 22) / d;
606  }
607 
608  if (accel_publisher_->trylock())
609  {
611  accel_publisher_->msg_.header.stamp = ros::Time::now();
612  accel_publisher_->msg_.samples.resize(count);
613  for (int i = 0; i < count; ++i)
614  {
615  accel_publisher_->msg_.samples[i].x = accelerometer_.state_.samples_[i].x;
616  accel_publisher_->msg_.samples[i].y = accelerometer_.state_.samples_[i].y;
617  accel_publisher_->msg_.samples[i].z = accelerometer_.state_.samples_[i].z;
618  }
620  }
621  return true;
622 }
623 
624 
646 void WG06::convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench)
647 {
648  // Apply gains / offset to F/T raw analog inputs
649  // Also, make sure values are within bounds.
650  // Out-of-bound values indicate a broken sensor or an overload.
651  double in[6];
652  for (unsigned i=0; i<6; ++i)
653  {
654  int raw_data = sample.data_[i];
655  if (abs(raw_data) > ft_overload_limit_)
656  {
657  ft_overload_flags_ |= (1<<i);
658  }
659  in[i] = (double(raw_data) - ft_params_.offset(i)) / ( ft_params_.gain(i) * double(1<<16) );
660  }
661 
662  // Vhalf ADC measurement should be amost half the ADC reference voltage
663  // For a 16bit ADC the Vhalf value should be about (1<<16)/2
664  // If Vhalf value is not close to this, this could mean 1 of 2 things:
665  // 1. WG035 electronics are damaged some-how
666  // 2. WG035 is not present or disconnected gripper MCB
667  if ( abs( int(sample.vhalf_) - FT_VHALF_IDEAL) > FT_VHALF_RANGE )
668  {
669  if ((sample.vhalf_ == 0x0000) || (sample.vhalf_ == 0xFFFF))
670  {
671  // When WG035 MCB is not present the DATA line floats low or high and all
672  // reads through SPI interface return 0.
673  ft_disconnected_ = true;
674  }
675  else
676  {
677  ft_vhalf_error_ = true;
678  }
679  }
680 
681  // Apply coeffiecient matrix multiplication to raw inputs to get force/torque values
682  double out[6];
683  for (unsigned i=0; i<6; ++i)
684  {
685  double sum=0.0;
686  for (unsigned j=0; j<6; ++j)
687  {
688  sum += ft_params_.calibration_coeff(i,j) * in[j];
689  }
690  out[i] = sum;
691  }
692 
693  wrench.force.x = out[0];
694  wrench.force.y = out[1];
695  wrench.force.z = out[2];
696  wrench.torque.x = out[3];
697  wrench.torque.y = out[4];
698  wrench.torque.z = out[5];
699 }
700 
701 
708 {
710 
711  ros::Time current_time(ros::Time::now());
712 
713  // Fill in raw analog output with most recent data sample, (might become deprecated?)
714  {
715  ft_raw_analog_in_.state_.state_.resize(6);
716  const FTDataSample &sample(status->ft_samples_[0]);
717  for (unsigned i=0; i<6; ++i)
718  {
719  int raw_data = sample.data_[i];
720  ft_raw_analog_in_.state_.state_[i] = double(raw_data);
721  }
722  }
723 
724  unsigned new_samples = (unsigned(status->ft_sample_count_) - unsigned(last_status->ft_sample_count_)) & 0xFF;
725  ft_sample_count_ += new_samples;
726  int missed_samples = std::max(int(0), int(new_samples) - int(MAX_FT_SAMPLES));
727  ft_missed_samples_ += missed_samples;
728  unsigned usable_samples = min(new_samples, MAX_FT_SAMPLES);
729 
730  // Also, if number of new_samples is ever 0, then there is also an error
731  if (usable_samples == 0)
732  {
734  }
735 
736  // Make room in data structure for more f/t samples
737  ft_state.samples_.resize(usable_samples);
738 
739  // add side "l" or "r" to frame_id
740  string ft_link_id = string(actuator_info_.name_).substr(0,1) + "_force_torque_link";
741 
742  // If any f/t channel is overload or the sampling rate is bad, there is an error.
743  ft_state.good_ = ( (!ft_sampling_rate_error_) &&
744  (ft_overload_flags_ == 0) &&
745  (!ft_disconnected_) &&
746  (!ft_vhalf_error_) );
747 
748  for (unsigned sample_index=0; sample_index<usable_samples; ++sample_index)
749  {
750  // samples are stored in status data, so that newest sample is at index 0.
751  // this is the reverse of the order data is stored in hardware_interface::ForceTorque buffer.
752  unsigned status_sample_index = usable_samples-sample_index-1;
753  const FTDataSample &sample(status->ft_samples_[status_sample_index]);
754  geometry_msgs::Wrench &wrench(ft_state.samples_[sample_index]);
755  convertFTDataSampleToWrench(sample, wrench);
756  }
757 
758  // Put newest sample into analog vector for controllers (deprecated)
759  if (usable_samples > 0)
760  {
761  const geometry_msgs::Wrench &wrench(ft_state.samples_[usable_samples-1]);
762  ft_analog_in_.state_.state_[0] = wrench.force.x;
763  ft_analog_in_.state_.state_[1] = wrench.force.y;
764  ft_analog_in_.state_.state_[2] = wrench.force.z;
765  ft_analog_in_.state_.state_[3] = wrench.torque.x;
766  ft_analog_in_.state_.state_[4] = wrench.torque.y;
767  ft_analog_in_.state_.state_[5] = wrench.torque.z;
768  }
769 
770  // Put all new samples in buffer and publish it
771  if ((raw_ft_publisher_ != NULL) && (raw_ft_publisher_->trylock()))
772  {
773  raw_ft_publisher_->msg_.samples.resize(usable_samples);
774  raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
775  raw_ft_publisher_->msg_.missed_samples = ft_missed_samples_;
776  for (unsigned sample_num=0; sample_num<usable_samples; ++sample_num)
777  {
778  // put data into message so oldest data is first element
779  const FTDataSample &sample(status->ft_samples_[sample_num]);
780  ethercat_hardware::RawFTDataSample &msg_sample(raw_ft_publisher_->msg_.samples[usable_samples-sample_num-1]);
781  msg_sample.sample_count = ft_sample_count_ - sample_num;
782  msg_sample.data.resize(NUM_FT_CHANNELS);
783  for (unsigned ch_num=0; ch_num<NUM_FT_CHANNELS; ++ch_num)
784  {
785  msg_sample.data[ch_num] = sample.data_[ch_num];
786  }
787  msg_sample.vhalf = sample.vhalf_;
788  }
789  raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
791  }
792 
793  // Put newest sample in realtime publisher
794  if ( (usable_samples > 0) && (ft_publisher_ != NULL) && (ft_publisher_->trylock()) )
795  {
796  ft_publisher_->msg_.header.stamp = current_time;
797  ft_publisher_->msg_.header.frame_id = ft_link_id;
798  ft_publisher_->msg_.wrench = ft_state.samples_[usable_samples-1];
800  }
801 
802  // If this returns false, it will cause motors to halt.
803  // Return "good_" state of sensor, unless halt_on_error is false.
804  return ft_state.good_ || !force_torque_.command_.halt_on_error_;
805 }
806 
807 
808 void WG06::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
809 {
811  diagnosticsWG06(d, buffer);
812  vec.push_back(d);
813  diagnosticsAccel(d, buffer);
814  vec.push_back(d);
815  diagnosticsPressure(d, buffer);
816  vec.push_back(d);
817 
819  {
821  // perform f/t sample
823  vec.push_back(d);
824  }
825 
827  first_publish_ = false;
828 }
829 
830 
832 {
833  stringstream str;
834  str << "Accelerometer (" << actuator_info_.name_ << ")";
835  d.name = str.str();
836  char serial[32];
837  snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
838  d.hardware_id = serial;
839 
840  d.summary(d.OK, "OK");
841  d.clear();
842 
844 
845  const char * range_str =
846  (acmd.range_ == 0) ? "+/-2G" :
847  (acmd.range_ == 1) ? "+/-4G" :
848  (acmd.range_ == 2) ? "+/-8G" :
849  "INVALID";
850 
851  const char * bandwidth_str =
852  (acmd.bandwidth_ == 6) ? "1500Hz" :
853  (acmd.bandwidth_ == 5) ? "750Hz" :
854  (acmd.bandwidth_ == 4) ? "375Hz" :
855  (acmd.bandwidth_ == 3) ? "190Hz" :
856  (acmd.bandwidth_ == 2) ? "100Hz" :
857  (acmd.bandwidth_ == 1) ? "50Hz" :
858  (acmd.bandwidth_ == 0) ? "25Hz" :
859  "INVALID";
860 
861  // Board revB=1 and revA=0 does not have accelerometer
862  bool has_accelerometer = (board_major_ >= 2);
863  double sample_frequency = 0.0;
864  ros::Time current_time(ros::Time::now());
865  if (!first_publish_)
866  {
867  sample_frequency = double(accelerometer_samples_) / (current_time - last_publish_time_).toSec();
868  {
869  if (((sample_frequency < 2000) || (sample_frequency > 4000)) && has_accelerometer)
870  {
871  d.mergeSummary(d.WARN, "Bad accelerometer sampling frequency");
872  }
873  }
874  }
876 
877  d.addf("Accelerometer", "%s", accelerometer_.state_.samples_.size() > 0 ? "Ok" : "Not Present");
878  d.addf("Accelerometer range", "%s (%d)", range_str, acmd.range_);
879  d.addf("Accelerometer bandwidth", "%s (%d)", bandwidth_str, acmd.bandwidth_);
880  d.addf("Accelerometer sample frequency", "%f", sample_frequency);
881  d.addf("Accelerometer missed samples", "%d", accelerometer_missed_samples_);
882 }
883 
884 
886 {
887  WG0X::diagnostics(d, buffer);
888  // nothing else to do here
889 }
890 
892 {
893  int status_bytes =
894  has_accel_and_ft_ ? sizeof(WG06StatusWithAccelAndFT) : // Has FT sensor and accelerometer
896  sizeof(WG0XStatus);
897  WG06Pressure *pressure = (WG06Pressure *)(buffer + command_size_ + status_bytes);
898 
899  stringstream str;
900  str << "Pressure sensors (" << actuator_info_.name_ << ")";
901  d.name = str.str();
902  char serial[32];
903  snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
904  d.hardware_id = serial;
905  d.clear();
906 
908  {
909  d.summary(d.OK, "OK");
910  }
911  else
912  {
913  d.summary(d.OK, "Pressure sensor disabled by user");
914  }
915 
917  {
918  d.mergeSummary(d.ERROR, "Checksum error on pressure data");
919  }
920 
922  {
923  // look at pressure sensor value to detect any damaged cables, or possibly missing sensor
924  unsigned l_finger_good_count = 0;
925  unsigned r_finger_good_count = 0;
926 
927  for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
928  {
929  uint16_t l_data = pressure->l_finger_tip_[region_num];
930  if ((l_data != 0xFFFF) && (l_data != 0x0000))
931  {
932  ++l_finger_good_count;
933  }
934 
935  uint16_t r_data = pressure->r_finger_tip_[region_num];
936  if ((r_data != 0xFFFF) && (r_data != 0x0000))
937  {
938  ++r_finger_good_count;
939  }
940  }
941 
942  // if no pressure sensor regions are return acceptable data, then its possible that the
943  // pressure sensor is not connected at all. This is just a warning, because on many robots
944  // the pressure sensor may not be installed
945  if ((l_finger_good_count == 0) && (r_finger_good_count == 0))
946  {
947  d.mergeSummary(d.WARN, "Pressure sensors may not be connected");
948  }
949  else
950  {
951  // At least one pressure sensor seems to be present ...
952  if (l_finger_good_count == 0)
953  {
954  d.mergeSummary(d.WARN, "Sensor on left finger may not be connected");
955  }
956  else if (l_finger_good_count < NUM_PRESSURE_REGIONS)
957  {
958  d.mergeSummary(d.WARN, "Sensor on left finger may have damaged sensor regions");
959  }
960 
961  if (r_finger_good_count == 0)
962  {
963  d.mergeSummary(d.WARN, "Sensor on right finger may not be connected");
964  }
965  else if (r_finger_good_count < NUM_PRESSURE_REGIONS)
966  {
967  d.mergeSummary(d.WARN, "Sensor on right finger may have damaged sensor regions");
968  }
969  }
970 
971  d.addf("Timestamp", "%u", pressure->timestamp_);
972  d.addf("Data size", "%u", pressure_size_);
973  d.addf("Checksum error count", "%u", pressure_checksum_error_count_);
974 
975  { // put right and left finger data in dianostics
976  std::stringstream ss;
977 
978  for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
979  {
980  ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
981  << pressure->r_finger_tip_[region_num] << " ";
982  if (region_num%8 == 7)
983  ss << std::endl;
984  }
985  d.add("Right finger data", ss.str());
986 
987  ss.str("");
988 
989  for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
990  {
991  ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
992  << pressure->l_finger_tip_[region_num] << " ";
993  if (region_num%8 == 7) ss << std::endl;
994  }
995  d.add("Left finger data", ss.str());
996  }
997  }
998 
999 }
1000 
1001 
1003 {
1004  stringstream str;
1005  str << "Force/Torque sensor (" << actuator_info_.name_ << ")";
1006  d.name = str.str();
1007  char serial[32];
1008  snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
1009  d.hardware_id = serial;
1010 
1011  d.summary(d.OK, "OK");
1012  d.clear();
1013 
1014  ros::Time current_time(ros::Time::now());
1015  double sample_frequency = 0.0;
1016  if (!first_publish_)
1017  {
1018  sample_frequency = double(ft_sample_count_ - diag_last_ft_sample_count_) / (current_time - last_publish_time_).toSec();
1019  }
1021 
1022  //d.addf("F/T sample count", "%llu", ft_sample_count_);
1023  d.addf("F/T sample frequency", "%.2f (Hz)", sample_frequency);
1024  d.addf("F/T missed samples", "%llu", ft_missed_samples_);
1025  std::stringstream ss;
1026  const FTDataSample &sample(status->ft_samples_[0]); //use newest data sample
1027  for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
1028  {
1029  ss.str(""); ss << "Ch"<< (i);
1030  d.addf(ss.str(), "%d", int(sample.data_[i]));
1031  }
1032  d.addf("FT Vhalf", "%d", int(sample.vhalf_));
1033 
1034  if (ft_overload_flags_ != 0)
1035  {
1036  d.mergeSummary(d.ERROR, "Sensor overloaded");
1037  ss.str("");
1038  for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
1039  {
1040  ss << "Ch" << i << " ";
1041  }
1042  }
1043  else
1044  {
1045  ss.str("None");
1046  }
1047  d.add("Overload Channels", ss.str());
1048 
1050  {
1051  d.mergeSummary(d.ERROR, "Sampling rate error");
1052  }
1053 
1054  if (ft_disconnected_)
1055  {
1056  d.mergeSummary(d.ERROR, "Amplifier disconnected");
1057  }
1058  else if (ft_vhalf_error_)
1059  {
1060  d.mergeSummary(d.ERROR, "Vhalf error, amplifier circuity may be damaged");
1061  }
1062 
1063  const std::vector<double> &ft_data( ft_analog_in_.state_.state_ );
1064  if (ft_data.size() == 6)
1065  {
1066  d.addf("Force X", "%f", ft_data[0]);
1067  d.addf("Force Y", "%f", ft_data[1]);
1068  d.addf("Force Z", "%f", ft_data[2]);
1069  d.addf("Torque X", "%f", ft_data[3]);
1070  d.addf("Torque Y", "%f", ft_data[4]);
1071  d.addf("Torque Z", "%f", ft_data[5]);
1072  }
1073 }
1074 
1075 
1076 
1078 {
1079  // Initial offset = 0.0
1080  // Gains = 1.0
1081  // Calibration coeff = identity matrix
1082  for (int i=0; i<6; ++i)
1083  {
1084  offset(i) = 0.0;
1085  gain(i) = 1.0;
1086  for (int j=0; j<6; ++j)
1087  {
1088  calibration_coeff(i,j) = (i==j) ? 1.0 : 0.0;
1089  }
1090  }
1091 }
1092 
1093 
1095 {
1096  for (int i=0; i<6; ++i)
1097  {
1098  ROS_INFO("offset[%d] = %f", i, offset(i));
1099  }
1100  for (int i=0; i<6; ++i)
1101  {
1102  ROS_INFO("gain[%d] = %f", i, gain(i));
1103  }
1104  for (int i=0; i<6; ++i)
1105  {
1106  ROS_INFO("coeff[%d] = [%f,%f,%f,%f,%f,%f]", i,
1110  );
1111  }
1112 }
1113 
1114 
1115 bool FTParamsInternal::getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len)
1116 {
1117  if(!params.hasMember(name))
1118  {
1119  ROS_ERROR("Expected ft_param to have '%s' element", name);
1120  return false;
1121  }
1122 
1123  XmlRpc::XmlRpcValue values = params[name];
1124  if (values.getType() != XmlRpc::XmlRpcValue::TypeArray)
1125  {
1126  ROS_ERROR("Expected FT param '%s' to be list type", name);
1127  return false;
1128  }
1129  if (values.size() != int(len))
1130  {
1131  ROS_ERROR("Expected FT param '%s' to have %d elements", name, len);
1132  return false;
1133  }
1134  for (unsigned i=0; i<len; ++i)
1135  {
1136  if (values[i].getType() != XmlRpc::XmlRpcValue::TypeDouble)
1137  {
1138  ROS_ERROR("Expected FT param %s[%d] to be floating point type", name, i);
1139  return false;
1140  } else {
1141  results[i] = static_cast<double>(values[i]);
1142  }
1143  }
1144 
1145  return true;
1146 }
1147 
1148 
1159 {
1160  if (!nh.hasParam("ft_params"))
1161  {
1162  ROS_WARN("'ft_params' not available for force/torque sensor in namespace '%s'",
1163  nh.getNamespace().c_str());
1164  return false;
1165  }
1166 
1167  XmlRpc::XmlRpcValue params;
1168  nh.getParam("ft_params", params);
1169  if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
1170  {
1171  ROS_ERROR("expected ft_params to be struct type");
1172  return false;
1173  }
1174 
1175  if (!getDoubleArray(params, "offsets", offsets_, 6))
1176  {
1177  return false;
1178  }
1179 
1180  if (!getDoubleArray(params, "gains", gains_, 6))
1181  {
1182  return false;
1183  }
1184 
1185  XmlRpc::XmlRpcValue coeff_matrix = params["calibration_coeff"];
1186  if (coeff_matrix.getType() != XmlRpc::XmlRpcValue::TypeArray)
1187  {
1188  ROS_ERROR("Expected FT param 'calibration_coeff' to be list type");
1189  return false;
1190  }
1191  if (coeff_matrix.size() != 6)
1192  {
1193  ROS_ERROR("Expected FT param 'calibration_coeff' to have 6 elements");
1194  return false;
1195  }
1196 
1197  for (int i=0; i<6; ++i)
1198  {
1199  XmlRpc::XmlRpcValue coeff_row = coeff_matrix[i];
1200  if (coeff_row.getType() != XmlRpc::XmlRpcValue::TypeArray)
1201  {
1202  ROS_ERROR("Expected FT param calibration_coeff[%d] to be list type", i);
1203  return false;
1204  }
1205  if (coeff_row.size() != 6)
1206  {
1207  ROS_ERROR("Expected FT param calibration_coeff[%d] to have 6 elements", i);
1208  return false;
1209  }
1210 
1211  for (int j=0; j<6; ++j)
1212  {
1213  if (coeff_row[j].getType() != XmlRpc::XmlRpcValue::TypeDouble)
1214  {
1215  ROS_ERROR("Expected FT param calibration_coeff[%d,%d] to be floating point type", i,j);
1216  return false;
1217  } else {
1218  calibration_coeff(i,j) = static_cast<double>(coeff_row[j]);
1219  }
1220  }
1221  }
1222 
1223  return true;
1224 }
1225 
1226 
1227 const unsigned WG06::NUM_FT_CHANNELS;
1228 const unsigned WG06::MAX_FT_SAMPLES;
WG06::diagnosticsPressure
void diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
Definition: wg06.cpp:891
XmlRpc::XmlRpcValue::size
int size() const
WG06::last_pressure_time_
uint32_t last_pressure_time_
Definition: wg06.h:213
WG06::initialize
int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
Definition: wg06.cpp:203
WG06::WG06
WG06()
Definition: wg06.cpp:56
WG06::unpackPressure
bool unpackPressure(unsigned char *pressure_buf)
Unpack pressure sensor samples from realtime data.
Definition: wg06.cpp:522
WG0X::verifyChecksum
bool verifyChecksum(const void *buffer, unsigned size)
Definition: wg0x.cpp:665
FTParamsInternal::getRosParams
bool getRosParams(ros::NodeHandle nh)
Grabs ft rosparams from a given node hande namespace.
Definition: wg06.cpp:1158
WG06::enable_ft_sensor_
bool enable_ft_sensor_
Definition: wg06.h:243
WG06::FT_VHALF_IDEAL
static const int FT_VHALF_IDEAL
Vhalf ADC measurement is ideally about (1<<16)/2.
Definition: wg06.h:220
WG06::pressure_checksum_error_
bool pressure_checksum_error_
Set true where checksum error on pressure data is detected, cleared on reset.
Definition: wg06.h:203
pr2_hardware_interface::Accelerometer::name_
std::string name_
WG0X::unpackState
bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
Definition: wg0x.cpp:614
WG0XConfigInfo::device_serial_number_
uint32_t device_serial_number_
Definition: wg0x.h:110
pr2_hardware_interface::AccelerometerState::samples_
std::vector< geometry_msgs::Vector3 > samples_
WG06::first_publish_
bool first_publish_
Definition: wg06.h:210
WG06::raw_ft_publisher_
realtime_tools::RealtimePublisher< ethercat_hardware::RawFTData > * raw_ft_publisher_
Realtime Publisher of RAW F/T data.
Definition: wg06.h:237
ROS_ERROR_STREAM
#define ROS_ERROR_STREAM(args)
FTParamsInternal::gain
const double & gain(unsigned ch_num) const
Definition: wg06.h:97
WG06Pressure
Definition: wg06.h:142
WG06::unpackAccel
bool unpackAccel(WG06StatusWithAccel *status, WG06StatusWithAccel *last_status)
Unpack 3-axis accelerometer samples from realtime data.
Definition: wg06.cpp:587
WG0X::actuator_info_
WG0XActuatorInfo actuator_info_
Definition: wg0x.h:263
FTDataSample::SIZE
static const unsigned SIZE
Definition: wg06.h:82
WG06Pressure::r_finger_tip_
uint16_t r_finger_tip_[22]
Definition: wg06.h:146
ethercat_hardware::WGSoftProcessor::initialize
bool initialize(EthercatCom *com)
Definition: wg_soft_processor.cpp:15
ros::NodeHandle::getParam
bool getParam(const std::string &key, bool &b) const
WG06::pressure_size_
unsigned pressure_size_
Size in bytes of pressure data region.
Definition: wg06.h:205
WG0XStatus
Definition: wg0x.h:160
ethercat_hardware::WGSoftProcessor::add
void add(WGMailbox *mbx, const std::string &actuator_name, const std::string &processor_name, unsigned iram_address, unsigned ctrl_address)
Definition: wg_soft_processor.cpp:26
EthercatDevice::use_ros_
bool use_ros_
Definition: ethercat_device.h:215
WG06Pressure::l_finger_tip_
uint16_t l_finger_tip_[22]
Definition: wg06.h:145
WG0XActuatorInfo::name_
char name_[64]
Definition: wg0x.h:141
WG0X::STATUS_PHY_ADDR
static const unsigned STATUS_PHY_ADDR
Definition: wg0x.h:357
WG06::ft_overload_limit_
int ft_overload_limit_
Limit on raw range of F/T input.
Definition: wg06.h:222
pr2_hardware_interface::PressureSensor::name_
std::string name_
EthercatDirectCom
Definition: ethercat_com.h:57
pr2_hardware_interface::ForceTorque::state_
ForceTorqueState state_
WG06::ft_vhalf_error_
bool ft_vhalf_error_
error with Vhalf reference voltage
Definition: wg06.h:225
WG0X::config_info_
WG0XConfigInfo config_info_
Definition: wg0x.h:264
FTParamsInternal::offset
const double & offset(unsigned ch_num) const
Definition: wg06.h:94
WG06::ft_params_
FTParamsInternal ft_params_
Definition: wg06.h:240
WG0X::actuator_
pr2_hardware_interface::Actuator actuator_
Definition: wg0x.h:270
WG06::unpackState
bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
Definition: wg06.cpp:461
pr2_hardware_interface::ForceTorqueState::good_
bool good_
WG06BigPressure::SIZE
static const unsigned SIZE
Definition: wg06.h:158
pr2_hardware_interface::AccelerometerCommand::range_
int range_
WG06::force_torque_
pr2_hardware_interface::ForceTorque force_torque_
Provides F/T data to controllers.
Definition: wg06.h:234
WG06StatusWithAccelAndFT::ft_sample_count_
uint8_t ft_sample_count_
Definition: wg06.h:134
WG06::multiDiagnostics
virtual void multiDiagnostics(vector< diagnostic_msgs::DiagnosticStatus > &vec, unsigned char *buffer)
For EtherCAT devices that publish more than one EtherCAT Status message. If sub-class implements mult...
Definition: wg06.cpp:808
WG06::accelerometer_missed_samples_
unsigned accelerometer_missed_samples_
Total of accelerometer samples that were missed.
Definition: wg06.h:208
WG06::accel_publisher_
realtime_tools::RealtimePublisher< pr2_msgs::AccelerometerState > * accel_publisher_
Definition: wg06.h:215
WG06::construct
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg06.cpp:90
pr2_hardware_interface::HardwareInterface
WG0X::status_checksum_error_
bool status_checksum_error_
Definition: wg0x.h:298
pr2_hardware_interface::AnalogIn::state_
AnalogInState state_
WG0XConfigInfo::product_id_
uint32_t product_id_
Definition: wg0x.h:98
WG06Pressure::SIZE
static const unsigned SIZE
Definition: wg06.h:149
XmlRpc::XmlRpcValue::TypeStruct
TypeStruct
FTDataSample::data_
int16_t data_[6]
Definition: wg06.h:78
WG06::accelerometer_
pr2_hardware_interface::Accelerometer accelerometer_
Definition: wg06.h:184
WG0XCommand
Definition: wg0x.h:187
EthercatDevice::sh_
EtherCAT_SlaveHandler * sh_
Definition: ethercat_device.h:217
WG06StatusWithAccel::SIZE
static const unsigned SIZE
Definition: wg06.h:104
WG06::enable_pressure_sensor_
bool enable_pressure_sensor_
Definition: wg06.h:242
FTParamsInternal::gains_
double gains_[6]
Definition: wg06.h:106
WG06Pressure::timestamp_
uint32_t timestamp_
Definition: wg06.h:144
WG06::unpackFT
bool unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status)
Unpack force/torque ADC samples from realtime data.
Definition: wg06.cpp:707
realtime_tools::RealtimePublisher< pr2_msgs::PressureState >
pr2_hardware_interface::ForceTorqueState
FTParamsInternal::offsets_
double offsets_[6]
Definition: wg06.h:105
WG06::diagnosticsAccel
void diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
Definition: wg06.cpp:831
WG06::pressure_checksum_error_count_
unsigned pressure_checksum_error_count_
debugging
Definition: wg06.h:204
WG06::initializeFT
bool initializeFT(pr2_hardware_interface::HardwareInterface *hw)
Definition: wg06.cpp:344
WG06::ft_missed_samples_
uint64_t ft_missed_samples_
Counts number of ft sensor samples that were missed.
Definition: wg06.h:228
wg06.h
pr2_hardware_interface::AccelerometerCommand::bandwidth_
int bandwidth_
XmlRpc::XmlRpcValue::TypeDouble
TypeDouble
WG06::PRESSURE_PHY_ADDR
static const unsigned PRESSURE_PHY_ADDR
Definition: wg06.h:180
WG0XCommand::checksum_
uint8_t checksum_
Definition: wg0x.h:194
WG06::MAX_FT_SAMPLES
static const unsigned MAX_FT_SAMPLES
Definition: wg06.h:218
WG0X::app_ram_status_
AppRamStatus app_ram_status_
Definition: wg0x.h:324
pr2_hardware_interface::DigitalOut::command_
DigitalOutCommand command_
WG06StatusWithAccel::accel_count_
uint8_t accel_count_
Definition: wg06.h:100
realtime_tools::RealtimePublisher::unlockAndPublish
void unlockAndPublish()
WG0X::PWM_MAX
static const int PWM_MAX
Definition: wg0x.h:347
WG0X::mailbox_
ethercat_hardware::WGMailbox mailbox_
Mailbox access to device.
Definition: wg0x.h:351
WG06::ft_raw_analog_in_
pr2_hardware_interface::AnalogIn ft_raw_analog_in_
Definition: wg06.h:230
EthercatDevice
Definition: ethercat_device.h:133
FTParamsInternal::calibration_coeff
const double & calibration_coeff(unsigned row, unsigned col) const
Definition: wg06.h:91
WG06::BIG_PRESSURE_PHY_ADDR
static const unsigned BIG_PRESSURE_PHY_ADDR
Definition: wg06.h:181
WG0X::digital_out_
pr2_hardware_interface::DigitalOut digital_out_
Definition: wg0x.h:271
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(WG06, EthercatDevice)
WG06::diagnosticsFT
void diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status)
Definition: wg06.cpp:1002
pr2_hardware_interface::HardwareInterface::addAccelerometer
bool addAccelerometer(Accelerometer *accelerometer)
WG06
Definition: wg06.h:163
ethercat_hardware::wg_util::computeChecksum
unsigned computeChecksum(void const *data, unsigned length)
Definition: wg_util.cpp:81
EthercatDevice::diagnostic_status_
diagnostic_updater::DiagnosticStatusWrapper diagnostic_status_
Definition: ethercat_device.h:233
WG06::packCommand
void packCommand(unsigned char *buffer, bool halt, bool reset)
Definition: wg06.cpp:431
WG06::diag_last_ft_sample_count_
uint64_t diag_last_ft_sample_count_
F/T Sample count last time diagnostics was published.
Definition: wg06.h:229
WG06::ft_sample_count_
uint64_t ft_sample_count_
Counts number of ft sensor samples.
Definition: wg06.h:227
realtime_tools::RealtimePublisher::msg_
Msg msg_
pr2_hardware_interface::PressureSensor::state_
PressureSensorState state_
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
WG06::diagnosticsWG06
void diagnosticsWG06(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
Definition: wg06.cpp:885
pr2_hardware_interface::AnalogInState::state_
std::vector< double > state_
d
d
realtime_tools::RealtimePublisher::trylock
bool trylock()
ROS_WARN
#define ROS_WARN(...)
name_
char * name_
Definition: motorconf.cpp:381
WG06::ft_sampling_rate_error_
bool ft_sampling_rate_error_
True if FT sampling rate was incorrect.
Definition: wg06.h:226
WG06::convertFTDataSampleToWrench
void convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench)
Convert FTDataSample to Wrench using gain, offset, and coefficient matrix.
Definition: wg06.cpp:646
FTParamsInternal
Definition: wg06.h:86
WG06::NUM_FT_CHANNELS
static const unsigned NUM_FT_CHANNELS
Definition: wg06.h:219
WG0X::fw_major_
uint8_t fw_major_
Definition: wg0x.h:258
WG06StatusWithAccel
Definition: wg06.h:47
min
static double min(double a, double b)
Definition: motor_model.cpp:4
WG06::initializeAccel
bool initializeAccel(pr2_hardware_interface::HardwareInterface *hw)
Definition: wg06.cpp:324
WG06::ft_disconnected_
bool ft_disconnected_
f/t sensor may be disconnected
Definition: wg06.h:224
wg_util.h
XmlRpc::XmlRpcValue::getType
const Type & getType() const
ROS_FATAL
#define ROS_FATAL(...)
FTDataSample::vhalf_
uint16_t vhalf_
Definition: wg06.h:79
pr2_hardware_interface::ForceTorqueState::samples_
std::vector< geometry_msgs::Wrench > samples_
pr2_hardware_interface::DigitalOutCommand::data_
uint8_t data_
XmlRpc::XmlRpcValue::TypeArray
TypeArray
pr2_hardware_interface::AccelerometerState::frame_id_
std::string frame_id_
WG06::initializePressure
bool initializePressure(pr2_hardware_interface::HardwareInterface *hw)
Definition: wg06.cpp:300
pr2_hardware_interface::PressureSensorState::data_
std::vector< uint16_t > data_
pr2_hardware_interface::ForceTorque::name_
std::string name_
WG06::ft_analog_in_
pr2_hardware_interface::AnalogIn ft_analog_in_
Provides F/T data to controllers (deprecated, use pr2_hardware_interface::ForceTorque instead)
Definition: wg06.h:232
WG06::FT_VHALF_RANGE
static const int FT_VHALF_RANGE
allow vhalf to range +/- 300 from ideal
Definition: wg06.h:221
XmlRpc::XmlRpcValue::hasMember
bool hasMember(const std::string &name) const
WG0X::diagnostics
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
For EtherCAT device that only publish one EtherCAT Status message. If sub-class implements multiDiagn...
Definition: wg0x.cpp:1324
ft_sample_count_
uint8_t ft_sample_count_
Definition: wg06.h:24
status
uint16_t status
Definition: ethercat_device.h:2
ros::Time
pr2_hardware_interface::ForceTorque::command_
ForceTorqueCommand command_
pr2_hardware_interface::AccelerometerCommand
WG06StatusWithAccelAndFT::SIZE
static const unsigned SIZE
Definition: wg06.h:138
WG06::has_accel_and_ft_
bool has_accel_and_ft_
True if device has accelerometer and force/torque sensor.
Definition: wg06.h:201
EthercatDevice::status_size_
unsigned int status_size_
Definition: ethercat_device.h:219
WG06::ft_publisher_
realtime_tools::RealtimePublisher< geometry_msgs::WrenchStamped > * ft_publisher_
Definition: wg06.h:238
WG0X::construct
void construct(EtherCAT_SlaveHandler *sh, int &start_address)
< Construct EtherCAT device
Definition: wg0x.cpp:198
FTParamsInternal::FTParamsInternal
FTParamsInternal()
Definition: wg06.cpp:1077
WG06StatusWithAccelAndFT
Definition: wg06.h:110
ROS_ERROR
#define ROS_ERROR(...)
WG0X::board_major_
uint8_t board_major_
Printed circuit board revision (for this value 0=='A', 1=='B')
Definition: wg0x.h:260
pr2_hardware_interface::Accelerometer::state_
AccelerometerState state_
pr2_hardware_interface::HardwareInterface::addAnalogIn
bool addAnalogIn(AnalogIn *analog_in)
pr2_hardware_interface::Accelerometer::command_
AccelerometerCommand command_
WG0X::APP_RAM_PRESENT
@ APP_RAM_PRESENT
Definition: wg0x.h:323
diagnostic_updater::DiagnosticStatusWrapper
WG06::soft_processor_
WGSoftProcessor soft_processor_
Definition: wg06.h:251
WG0X::writeMailbox
int writeMailbox(EthercatCom *com, unsigned address, void const *data, unsigned length)
Write data to WG0X local bus using mailbox communication.
Definition: wg0x.cpp:1156
pr2_hardware_interface::HardwareInterface::addForceTorque
bool addForceTorque(ForceTorque *forcetorque)
WG06::last_publish_time_
ros::Time last_publish_time_
Time diagnostics was last published.
Definition: wg06.h:209
WG06::pressure_sensors_
pr2_hardware_interface::PressureSensor pressure_sensors_[2]
Definition: wg06.h:183
WG06::NUM_PRESSURE_REGIONS
static const unsigned NUM_PRESSURE_REGIONS
Definition: wg06.h:212
WG0X::initialize
virtual int initialize(pr2_hardware_interface::HardwareInterface *, bool allow_unprogrammed=true)
Definition: wg0x.cpp:395
WG06::initializeSoftProcessor
bool initializeSoftProcessor()
Definition: wg06.cpp:410
EthercatDevice::command_size_
unsigned int command_size_
Definition: ethercat_device.h:218
ros::NodeHandle::getNamespace
const std::string & getNamespace() const
WG0X::initializeMotorModel
bool initializeMotorModel(pr2_hardware_interface::HardwareInterface *hw, const string &device_description, double max_pwm_ratio, double board_resistance, bool poor_measured_motor_voltage)
Allocates and initialized motor trace for WG0X devices than use it (WG006, WG005)
Definition: wg0x.cpp:238
WG06::ft_overload_flags_
uint8_t ft_overload_flags_
Bits 0-5 set to true if raw FT input goes beyond limit.
Definition: wg06.h:223
pr2_hardware_interface::Actuator::name_
std::string name_
WG06::pressure_publisher_
realtime_tools::RealtimePublisher< pr2_msgs::PressureState > * pressure_publisher_
Definition: wg06.h:214
WG0XCommand::digital_out_
uint8_t digital_out_
Definition: wg0x.h:190
ROS_INFO
#define ROS_INFO(...)
pr2_hardware_interface::HardwareInterface::addPressureSensor
bool addPressureSensor(PressureSensor *sensor)
WG06::~WG06
~WG06()
Definition: wg06.cpp:84
WG0X::packCommand
void packCommand(unsigned char *buffer, bool halt, bool reset)
Definition: wg0x.cpp:563
WG0X::COMMAND_PHY_ADDR
static const unsigned COMMAND_PHY_ADDR
Definition: wg0x.h:356
WG06::accelerometer_samples_
unsigned accelerometer_samples_
Number of accelerometer samples since last publish cycle.
Definition: wg06.h:207
WG06BigPressure
Definition: wg06.h:153
WG0X::fw_minor_
uint8_t fw_minor_
Definition: wg0x.h:259
pr2_hardware_interface::ForceTorqueCommand::halt_on_error_
bool halt_on_error_
ethercat_hardware::wg_util::rotateRight8
unsigned int rotateRight8(unsigned in)
Definition: wg_util.cpp:73
FTParamsInternal::getDoubleArray
bool getDoubleArray(XmlRpc::XmlRpcValue params, const char *name, double *results, unsigned len)
Definition: wg06.cpp:1115
FTParamsInternal::print
void print() const
Definition: wg06.cpp:1094
WG06::enable_soft_processor_access_
bool enable_soft_processor_access_
Definition: wg06.h:250
XmlRpc::XmlRpcValue
ros::NodeHandle
FTDataSample
Definition: wg06.h:76
ros::Time::now
static Time now()
pr2_hardware_interface::AnalogIn::name_
std::string name_


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Sep 26 2024 02:44:04