00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <iomanip>
00036
00037 #include <math.h>
00038 #include <stddef.h>
00039
00040 #include <ethercat_hardware/wg06.h>
00041
00042 #include <dll/ethercat_dll.h>
00043 #include <al/ethercat_AL.h>
00044 #include <dll/ethercat_device_addressed_telegram.h>
00045 #include <dll/ethercat_frame.h>
00046
00047 #include <boost/static_assert.hpp>
00048
00049 #include "ethercat_hardware/wg_util.h"
00050
00051 PLUGINLIB_DECLARE_CLASS(ethercat_hardware, 6805006, WG06, EthercatDevice);
00052
00053
00054 WG06::WG06() :
00055 has_accel_and_ft_(false),
00056 pressure_checksum_error_(false),
00057 pressure_checksum_error_count_(0),
00058 accelerometer_samples_(0),
00059 accelerometer_missed_samples_(0),
00060 first_publish_(true),
00061 last_pressure_time_(0),
00062 pressure_publisher_(NULL),
00063 accel_publisher_(NULL),
00064 ft_overload_limit_(31100),
00065 ft_overload_flags_(0),
00066 ft_disconnected_(false),
00067 ft_vhalf_error_(false),
00068 ft_sampling_rate_error_(false),
00069 ft_sample_count_(0),
00070 ft_missed_samples_(0),
00071 diag_last_ft_sample_count_(0),
00072 raw_ft_publisher_(NULL),
00073 ft_publisher_(NULL),
00074 enable_pressure_sensor_(true),
00075 enable_ft_sensor_(false),
00076 enable_soft_processor_access_(true)
00077
00078 {
00079
00080 }
00081
00082 WG06::~WG06()
00083 {
00084 if (pressure_publisher_) delete pressure_publisher_;
00085 if (accel_publisher_) delete accel_publisher_;
00086 }
00087
00088 void WG06::construct(EtherCAT_SlaveHandler *sh, int &start_address)
00089 {
00090 WG0X::construct(sh, start_address);
00091
00092 has_accel_and_ft_ = false;
00093
00094
00095 BOOST_STATIC_ASSERT(sizeof(WG06StatusWithAccel) == WG06StatusWithAccel::SIZE);
00096 BOOST_STATIC_ASSERT(sizeof(FTDataSample) == FTDataSample::SIZE);
00097 BOOST_STATIC_ASSERT(sizeof(WG06Pressure) == WG06Pressure::SIZE);
00098 BOOST_STATIC_ASSERT(sizeof(WG06BigPressure) == WG06BigPressure::SIZE);
00099 BOOST_STATIC_ASSERT(sizeof(WG06StatusWithAccelAndFT) == WG06StatusWithAccelAndFT::SIZE);
00100
00101 unsigned int base_status_size = sizeof(WG0XStatus);
00102
00103 command_size_ = sizeof(WG0XCommand);
00104 status_size_ = sizeof(WG0XStatus);
00105 pressure_size_ = sizeof(WG06Pressure);
00106 unsigned pressure_phy_addr = PRESSURE_PHY_ADDR;
00107
00108 if (fw_major_ == 0)
00109 {
00110
00111 }
00112 if (fw_major_ == 1)
00113 {
00114
00115 status_size_ = base_status_size = sizeof(WG06StatusWithAccel);
00116 }
00117 else if ((fw_major_ == 2) || (fw_major_ == 3))
00118 {
00119
00120 status_size_ = base_status_size = sizeof(WG06StatusWithAccelAndFT);
00121 has_accel_and_ft_ = true;
00122 if (fw_major_ == 3)
00123 {
00124
00125 pressure_size_ = sizeof(WG06BigPressure);
00126 pressure_phy_addr = BIG_PRESSURE_PHY_ADDR;
00127 }
00128 }
00129 else
00130 {
00131 ROS_ERROR("Unsupported WG06 FW major version %d", fw_major_);
00132 }
00133 status_size_ += pressure_size_;
00134
00135
00136 EtherCAT_FMMU_Config *fmmu = new EtherCAT_FMMU_Config(3);
00137
00138 (*fmmu)[0] = EC_FMMU(start_address,
00139 command_size_,
00140 0x00,
00141 0x07,
00142 COMMAND_PHY_ADDR,
00143 0x00,
00144 false,
00145 true,
00146 true);
00147
00148 start_address += command_size_;
00149
00150
00151 (*fmmu)[1] = EC_FMMU(start_address,
00152 base_status_size,
00153 0x00,
00154 0x07,
00155 STATUS_PHY_ADDR,
00156 0x00,
00157 true,
00158 false,
00159 true);
00160
00161 start_address += base_status_size;
00162
00163 (*fmmu)[2] = EC_FMMU(start_address,
00164 pressure_size_,
00165 0x00,
00166 0x07,
00167 pressure_phy_addr,
00168 0x00,
00169 true,
00170 false,
00171 true);
00172
00173 start_address += pressure_size_;
00174
00175 sh->set_fmmu_config(fmmu);
00176
00177 EtherCAT_PD_Config *pd = new EtherCAT_PD_Config(5);
00178
00179
00180 (*pd)[0] = EC_SyncMan(COMMAND_PHY_ADDR, command_size_, EC_BUFFERED, EC_WRITTEN_FROM_MASTER);
00181 (*pd)[0].ChannelEnable = true;
00182 (*pd)[0].ALEventEnable = true;
00183
00184 (*pd)[1] = EC_SyncMan(STATUS_PHY_ADDR, base_status_size);
00185 (*pd)[1].ChannelEnable = true;
00186
00187 (*pd)[2] = EC_SyncMan(WGMailbox::MBX_COMMAND_PHY_ADDR, WGMailbox::MBX_COMMAND_SIZE, EC_QUEUED, EC_WRITTEN_FROM_MASTER);
00188 (*pd)[2].ChannelEnable = true;
00189 (*pd)[2].ALEventEnable = true;
00190
00191 (*pd)[3] = EC_SyncMan(WGMailbox::MBX_STATUS_PHY_ADDR, WGMailbox::MBX_STATUS_SIZE, EC_QUEUED);
00192 (*pd)[3].ChannelEnable = true;
00193
00194 (*pd)[4] = EC_SyncMan(pressure_phy_addr, pressure_size_);
00195 (*pd)[4].ChannelEnable = true;
00196
00197 sh->set_pd_config(pd);
00198 }
00199
00200
00201 int WG06::initialize(pr2_hardware_interface::HardwareInterface *hw, bool allow_unprogrammed)
00202 {
00203 if ( ((fw_major_ == 1) && (fw_minor_ >= 1)) || (fw_major_ >= 2) )
00204 {
00205 app_ram_status_ = APP_RAM_PRESENT;
00206 }
00207
00208 int retval = WG0X::initialize(hw, allow_unprogrammed);
00209
00210 if (!retval && use_ros_)
00211 {
00212 bool poor_measured_motor_voltage = false;
00213 double max_pwm_ratio = double(0x2700) / double(PWM_MAX);
00214 double board_resistance = 5.0;
00215 if (!WG0X::initializeMotorModel(hw, "WG006", max_pwm_ratio, board_resistance, poor_measured_motor_voltage))
00216 {
00217 ROS_FATAL("Initializing motor trace failed");
00218 sleep(1);
00219 return -1;
00220 }
00221
00222
00223 ros::NodeHandle nh(string("~/") + actuator_.name_);
00224 bool pressure_enabled_ ;
00225 if (!nh.getParam("enable_pressure_sensor", enable_pressure_sensor_))
00226 {
00227 pressure_enabled_ = true;
00228 }
00229 if (!nh.getParam("enable_ft_sensor", enable_ft_sensor_))
00230 {
00231 enable_ft_sensor_ = false;
00232 }
00233
00234 if (enable_ft_sensor_ && (fw_major_ < 2))
00235 {
00236 ROS_WARN("Gripper firmware version %d does not support enabling force/torque sensor", fw_major_);
00237 enable_ft_sensor_ = false;
00238 }
00239
00240
00241 if (fw_major_ >= 2)
00242 {
00243 static const uint8_t PRESSURE_ENABLE_FLAG = 0x1;
00244 static const uint8_t FT_ENABLE_FLAG = 0x2;
00245 static const unsigned PRESSURE_FT_ENABLE_ADDR = 0xAA;
00246 uint8_t pressure_ft_enable = 0;
00247 if (enable_pressure_sensor_) pressure_ft_enable |= PRESSURE_ENABLE_FLAG;
00248 if (enable_ft_sensor_) pressure_ft_enable |= FT_ENABLE_FLAG;
00249 EthercatDirectCom com(EtherCAT_DataLinkLayer::instance());
00250 if (writeMailbox(&com, PRESSURE_FT_ENABLE_ADDR, &pressure_ft_enable, 1) != 0)
00251 {
00252 ROS_FATAL("Could not enable/disable pressure and force/torque sensors");
00253 return -1;
00254 }
00255 }
00256
00257 if (!initializePressure(hw))
00258 {
00259 return -1;
00260 }
00261
00262
00263 if (fw_major_ >= 1)
00264 {
00265 if (!initializeAccel(hw))
00266 {
00267 return -1;
00268 }
00269 }
00270
00271
00272
00273 if ((fw_major_ >= 2) && (enable_ft_sensor_))
00274 {
00275 if (!initializeFT(hw))
00276 {
00277 return -1;
00278 }
00279 }
00280
00281
00282
00283 if ((fw_major_ >= 2) && enable_soft_processor_access_)
00284 {
00285 if (!initializeSoftProcessor())
00286 {
00287 return -1;
00288 }
00289 }
00290
00291
00292 }
00293
00294 return retval;
00295 }
00296
00297
00298 bool WG06::initializePressure(pr2_hardware_interface::HardwareInterface *hw)
00299 {
00300
00301 string topic = "pressure";
00302 if (!actuator_.name_.empty())
00303 topic = topic + "/" + string(actuator_.name_);
00304 pressure_publisher_ = new realtime_tools::RealtimePublisher<pr2_msgs::PressureState>(ros::NodeHandle(), topic, 1);
00305
00306
00307 for (int i = 0; i < 2; ++i)
00308 {
00309 pressure_sensors_[i].state_.data_.resize(22);
00310 pressure_sensors_[i].name_ = string(actuator_info_.name_) + string(i ? "r_finger_tip" : "l_finger_tip");
00311 if (hw && !hw->addPressureSensor(&pressure_sensors_[i]))
00312 {
00313 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());
00314 return false;
00315 }
00316 }
00317
00318 return true;
00319 }
00320
00321
00322 bool WG06::initializeAccel(pr2_hardware_interface::HardwareInterface *hw)
00323 {
00324 string topic = "accelerometer";
00325 if (!actuator_.name_.empty())
00326 {
00327 topic = topic + "/" + string(actuator_.name_);
00328 }
00329 accel_publisher_ = new realtime_tools::RealtimePublisher<pr2_msgs::AccelerometerState>(ros::NodeHandle(), topic, 1);
00330
00331
00332 accelerometer_.name_ = actuator_info_.name_;
00333 if (hw && !hw->addAccelerometer(&accelerometer_))
00334 {
00335 ROS_FATAL("An accelerometer of the name '%s' already exists. Device #%02d has a duplicate name", accelerometer_.name_.c_str(), sh_->get_ring_position());
00336 return false;
00337 }
00338 return true;
00339 }
00340
00341
00342 bool WG06::initializeFT(pr2_hardware_interface::HardwareInterface *hw)
00343 {
00344 ft_raw_analog_in_.name_ = actuator_.name_ + "_ft_raw";
00345 if (hw && !hw->addAnalogIn(&ft_raw_analog_in_))
00346 {
00347 ROS_FATAL("An analog in of the name '%s' already exists. Device #%02d has a duplicate name",
00348 ft_raw_analog_in_.name_.c_str(), sh_->get_ring_position());
00349 return false;
00350 }
00351
00352 ft_raw_analog_in_.state_.state_.resize(6);
00353
00354 force_torque_.state_.samples_.reserve(4);
00355 force_torque_.state_.good_ = true;
00356
00357
00358 std::string topic = "raw_ft";
00359 if (!actuator_.name_.empty())
00360 topic = topic + "/" + string(actuator_.name_);
00361 raw_ft_publisher_ = new realtime_tools::RealtimePublisher<ethercat_hardware::RawFTData>(ros::NodeHandle(), topic, 1);
00362 if (raw_ft_publisher_ == NULL)
00363 {
00364 ROS_FATAL("Could not allocate raw_ft publisher");
00365 return false;
00366 }
00367
00368 raw_ft_publisher_->msg_.samples.reserve(MAX_FT_SAMPLES);
00369
00370 force_torque_.command_.halt_on_error_ = false;
00371 force_torque_.state_.good_ = true;
00372
00373 if (!actuator_.name_.empty())
00374 {
00375 ft_analog_in_.state_.state_.resize(6);
00376 ros::NodeHandle nh("~" + string(actuator_.name_));
00377 FTParamsInternal ft_params;
00378 if ( ft_params.getRosParams(nh) )
00379 {
00380 ft_params_ = ft_params;
00381 ft_params_.print();
00382
00383 topic = "ft";
00384 if (!actuator_.name_.empty())
00385 topic = topic + "/" + string(actuator_.name_);
00386 ft_publisher_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(ros::NodeHandle(), topic, 1);
00387 if (ft_publisher_ == NULL)
00388 {
00389 ROS_FATAL("Could not allocate ft publisher");
00390 return false;
00391 }
00392 }
00393 }
00394
00395 return true;
00396 }
00397
00398
00399 bool WG06::initializeSoftProcessor()
00400 {
00401
00402
00403 EthercatDirectCom *com = new EthercatDirectCom(EtherCAT_DataLinkLayer::instance());
00404
00405
00406 soft_processor_.add(&mailbox_, actuator_.name_, "pressure", 0xA000, 0x249);
00407 soft_processor_.add(&mailbox_, actuator_.name_, "accel", 0xB000, 0x24A);
00408
00409
00410 if (!soft_processor_.initialize(com))
00411 {
00412 return false;
00413 }
00414
00415 return true;
00416 }
00417
00418
00419
00420 void WG06::packCommand(unsigned char *buffer, bool halt, bool reset)
00421 {
00422 if (reset)
00423 {
00424 pressure_checksum_error_ = false;
00425 ft_overload_flags_ = 0;
00426 ft_disconnected_ = false;
00427 ft_vhalf_error_ = false;
00428 ft_sampling_rate_error_ = false;
00429 }
00430
00431 WG0X::packCommand(buffer, halt, reset);
00432
00433 WG0XCommand *c = (WG0XCommand *)buffer;
00434
00435 if (accelerometer_.command_.range_ > 2 ||
00436 accelerometer_.command_.range_ < 0)
00437 accelerometer_.command_.range_ = 0;
00438
00439 if (accelerometer_.command_.bandwidth_ > 6 ||
00440 accelerometer_.command_.bandwidth_ < 0)
00441 accelerometer_.command_.bandwidth_ = 0;
00442
00443 c->digital_out_ = (digital_out_.command_.data_ != 0) |
00444 ((accelerometer_.command_.bandwidth_ & 0x7) << 1) |
00445 ((accelerometer_.command_.range_ & 0x3) << 4);
00446 c->checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(c, command_size_ - 1));
00447 }
00448
00449
00450 bool WG06::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00451 {
00452 bool rv = true;
00453
00454 int status_bytes =
00455 has_accel_and_ft_ ? sizeof(WG06StatusWithAccelAndFT) :
00456 accel_publisher_ ? sizeof(WG06StatusWithAccel) :
00457 sizeof(WG0XStatus);
00458
00459 unsigned char *pressure_buf = (this_buffer + command_size_ + status_bytes);
00460
00461 unsigned char* this_status = this_buffer + command_size_;
00462 if (!verifyChecksum(this_status, status_bytes))
00463 {
00464 status_checksum_error_ = true;
00465 rv = false;
00466 goto end;
00467 }
00468
00469 if (!unpackPressure(pressure_buf))
00470 {
00471 rv = false;
00472
00473 }
00474
00475 if (accel_publisher_)
00476 {
00477 WG06StatusWithAccel *status = (WG06StatusWithAccel *)(this_buffer + command_size_);
00478 WG06StatusWithAccel *last_status = (WG06StatusWithAccel *)(prev_buffer + command_size_);
00479 if (!unpackAccel(status, last_status))
00480 {
00481 rv=false;
00482 }
00483 }
00484
00485 if (has_accel_and_ft_ && enable_ft_sensor_)
00486 {
00487 WG06StatusWithAccelAndFT *status = (WG06StatusWithAccelAndFT *)(this_buffer + command_size_);
00488 WG06StatusWithAccelAndFT *last_status = (WG06StatusWithAccelAndFT *)(prev_buffer + command_size_);
00489 if (!unpackFT(status, last_status))
00490 {
00491 rv = false;
00492 }
00493 }
00494
00495
00496 if (!WG0X::unpackState(this_buffer, prev_buffer))
00497 {
00498 rv = false;
00499 }
00500
00501 end:
00502 return rv;
00503 }
00504
00505
00511 bool WG06::unpackPressure(unsigned char *pressure_buf)
00512 {
00513 if (!enable_pressure_sensor_)
00514 {
00515
00516 return true;
00517 }
00518
00519 if (!verifyChecksum(pressure_buf, pressure_size_))
00520 {
00521 ++pressure_checksum_error_count_;
00522 if (false )
00523 {
00524 std::stringstream ss;
00525 ss << "Pressure buffer checksum error : " << std::endl;
00526 for (unsigned ii=0; ii<pressure_size_; ++ii)
00527 {
00528 ss << std::uppercase << std::hex << std::setw(2) << std::setfill('0')
00529 << unsigned(pressure_buf[ii]) << " ";
00530 if ((ii%8) == 7) ss << std::endl;
00531 }
00532 ROS_ERROR_STREAM(ss.str());
00533 std::cerr << ss.str() << std::endl;
00534 }
00535 pressure_checksum_error_ = true;
00536 return false;
00537 }
00538 else
00539 {
00540 WG06Pressure *p( (WG06Pressure *) pressure_buf);
00541 for (int i = 0; i < 22; ++i ) {
00542 pressure_sensors_[0].state_.data_[i] =
00543 ((p->l_finger_tip_[i] >> 8) & 0xff) |
00544 ((p->l_finger_tip_[i] << 8) & 0xff00);
00545 pressure_sensors_[1].state_.data_[i] =
00546 ((p->r_finger_tip_[i] >> 8) & 0xff) |
00547 ((p->r_finger_tip_[i] << 8) & 0xff00);
00548 }
00549
00550 if (p->timestamp_ != last_pressure_time_)
00551 {
00552 if (pressure_publisher_ && pressure_publisher_->trylock())
00553 {
00554 pressure_publisher_->msg_.header.stamp = ros::Time::now();
00555 pressure_publisher_->msg_.l_finger_tip.resize(22);
00556 pressure_publisher_->msg_.r_finger_tip.resize(22);
00557 for (int i = 0; i < 22; ++i ) {
00558 pressure_publisher_->msg_.l_finger_tip[i] = pressure_sensors_[0].state_.data_[i];
00559 pressure_publisher_->msg_.r_finger_tip[i] = pressure_sensors_[1].state_.data_[i];
00560 }
00561 pressure_publisher_->unlockAndPublish();
00562 }
00563 }
00564 last_pressure_time_ = p->timestamp_;
00565 }
00566
00567 return true;
00568 }
00569
00570
00576 bool WG06::unpackAccel(WG06StatusWithAccel *status, WG06StatusWithAccel *last_status)
00577 {
00578 int count = uint8_t(status->accel_count_ - last_status->accel_count_);
00579 accelerometer_samples_ += count;
00580
00581
00582
00583 accelerometer_missed_samples_ += (count > 4) ? (count-4) : 0;
00584 count = min(4, count);
00585 accelerometer_.state_.samples_.resize(count);
00586 accelerometer_.state_.frame_id_ = string(actuator_info_.name_) + "_accelerometer_link";
00587 for (int i = 0; i < count; ++i)
00588 {
00589 int32_t acc = status->accel_[count - i - 1];
00590 int range = (acc >> 30) & 3;
00591 float d = 1 << (8 - range);
00592 accelerometer_.state_.samples_[i].x = 9.81 * ((((acc >> 0) & 0x3ff) << 22) >> 22) / d;
00593 accelerometer_.state_.samples_[i].y = 9.81 * ((((acc >> 10) & 0x3ff) << 22) >> 22) / d;
00594 accelerometer_.state_.samples_[i].z = 9.81 * ((((acc >> 20) & 0x3ff) << 22) >> 22) / d;
00595 }
00596
00597 if (accel_publisher_->trylock())
00598 {
00599 accel_publisher_->msg_.header.frame_id = accelerometer_.state_.frame_id_;
00600 accel_publisher_->msg_.header.stamp = ros::Time::now();
00601 accel_publisher_->msg_.samples.resize(count);
00602 for (int i = 0; i < count; ++i)
00603 {
00604 accel_publisher_->msg_.samples[i].x = accelerometer_.state_.samples_[i].x;
00605 accel_publisher_->msg_.samples[i].y = accelerometer_.state_.samples_[i].y;
00606 accel_publisher_->msg_.samples[i].z = accelerometer_.state_.samples_[i].z;
00607 }
00608 accel_publisher_->unlockAndPublish();
00609 }
00610 return true;
00611 }
00612
00613
00635 void WG06::convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench)
00636 {
00637
00638
00639
00640 double in[6];
00641 for (unsigned i=0; i<6; ++i)
00642 {
00643 int raw_data = sample.data_[i];
00644 if (abs(raw_data) > ft_overload_limit_)
00645 {
00646 ft_overload_flags_ |= (1<<i);
00647 }
00648 in[i] = (double(raw_data) - ft_params_.offset(i)) / ( ft_params_.gain(i) * double(1<<16) );
00649 }
00650
00651
00652
00653
00654
00655
00656 if ( abs( int(sample.vhalf_) - FT_VHALF_IDEAL) > FT_VHALF_RANGE )
00657 {
00658 if ((sample.vhalf_ == 0x0000) || (sample.vhalf_ == 0xFFFF))
00659 {
00660
00661
00662 ft_disconnected_ = true;
00663 }
00664 else
00665 {
00666 ft_vhalf_error_ = true;
00667 }
00668 }
00669
00670
00671 double out[6];
00672 for (unsigned i=0; i<6; ++i)
00673 {
00674 double sum=0.0;
00675 for (unsigned j=0; j<6; ++j)
00676 {
00677 sum += ft_params_.calibration_coeff(i,j) * in[j];
00678 }
00679 out[i] = sum;
00680 }
00681
00682 wrench.force.x = out[0];
00683 wrench.force.y = out[1];
00684 wrench.force.z = out[2];
00685 wrench.torque.x = out[3];
00686 wrench.torque.y = out[4];
00687 wrench.torque.z = out[5];
00688 }
00689
00690
00696 bool WG06::unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status)
00697 {
00698 pr2_hardware_interface::ForceTorqueState &ft_state(force_torque_.state_);
00699
00700 ros::Time current_time(ros::Time::now());
00701
00702
00703 {
00704 ft_raw_analog_in_.state_.state_.resize(6);
00705 const FTDataSample &sample(status->ft_samples_[0]);
00706 for (unsigned i=0; i<6; ++i)
00707 {
00708 int raw_data = sample.data_[i];
00709 ft_raw_analog_in_.state_.state_[i] = double(raw_data);
00710 }
00711 }
00712
00713 unsigned new_samples = (unsigned(status->ft_sample_count_) - unsigned(last_status->ft_sample_count_)) & 0xFF;
00714 ft_sample_count_ += new_samples;
00715 int missed_samples = std::max(int(0), int(new_samples) - 4);
00716 ft_missed_samples_ += missed_samples;
00717 unsigned usable_samples = min(new_samples, MAX_FT_SAMPLES);
00718
00719
00720 if (usable_samples == 0)
00721 {
00722 ft_sampling_rate_error_ = true;
00723 }
00724
00725
00726 ft_state.samples_.resize(usable_samples);
00727
00728
00729 ft_state.good_ = ( (!ft_sampling_rate_error_) &&
00730 (ft_overload_flags_ == 0) &&
00731 (!ft_disconnected_) &&
00732 (!ft_vhalf_error_) );
00733
00734 for (unsigned sample_index=0; sample_index<usable_samples; ++sample_index)
00735 {
00736
00737
00738 unsigned status_sample_index = usable_samples-sample_index-1;
00739 const FTDataSample &sample(status->ft_samples_[status_sample_index]);
00740 geometry_msgs::Wrench &wrench(ft_state.samples_[sample_index]);
00741 convertFTDataSampleToWrench(sample, wrench);
00742 }
00743
00744
00745 if (usable_samples > 0)
00746 {
00747 const geometry_msgs::Wrench &wrench(ft_state.samples_[usable_samples-1]);
00748 ft_analog_in_.state_.state_[0] = wrench.force.x;
00749 ft_analog_in_.state_.state_[1] = wrench.force.y;
00750 ft_analog_in_.state_.state_[2] = wrench.force.z;
00751 ft_analog_in_.state_.state_[3] = wrench.torque.x;
00752 ft_analog_in_.state_.state_[4] = wrench.torque.y;
00753 ft_analog_in_.state_.state_[5] = wrench.torque.z;
00754 }
00755
00756
00757 if ((raw_ft_publisher_ != NULL) && (raw_ft_publisher_->trylock()))
00758 {
00759 raw_ft_publisher_->msg_.samples.resize(usable_samples);
00760 raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
00761 raw_ft_publisher_->msg_.missed_samples = ft_missed_samples_;
00762 for (unsigned sample_num=0; sample_num<usable_samples; ++sample_num)
00763 {
00764
00765 const FTDataSample &sample(status->ft_samples_[sample_num]);
00766 ethercat_hardware::RawFTDataSample &msg_sample(raw_ft_publisher_->msg_.samples[usable_samples-sample_num-1]);
00767 msg_sample.sample_count = ft_sample_count_ - sample_num;
00768 msg_sample.data.resize(NUM_FT_CHANNELS);
00769 for (unsigned ch_num=0; ch_num<NUM_FT_CHANNELS; ++ch_num)
00770 {
00771 msg_sample.data[ch_num] = sample.data_[ch_num];
00772 }
00773 msg_sample.vhalf = sample.vhalf_;
00774 }
00775 raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
00776 raw_ft_publisher_->unlockAndPublish();
00777 }
00778
00779
00780 if ( (usable_samples > 0) && (ft_publisher_ != NULL) && (ft_publisher_->trylock()) )
00781 {
00782 ft_publisher_->msg_.header.stamp = current_time;
00783 ft_publisher_->msg_.wrench = ft_state.samples_[usable_samples-1];
00784 ft_publisher_->unlockAndPublish();
00785 }
00786
00787
00788
00789 return ft_state.good_ || !force_torque_.command_.halt_on_error_;
00790 }
00791
00792
00793 void WG06::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
00794 {
00795 diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);
00796 diagnosticsWG06(d, buffer);
00797 vec.push_back(d);
00798 diagnosticsAccel(d, buffer);
00799 vec.push_back(d);
00800 diagnosticsPressure(d, buffer);
00801 vec.push_back(d);
00802
00803 if (has_accel_and_ft_ && enable_ft_sensor_)
00804 {
00805 WG06StatusWithAccelAndFT *status = (WG06StatusWithAccelAndFT *)(buffer + command_size_);
00806
00807 diagnosticsFT(d, status);
00808 vec.push_back(d);
00809 }
00810
00811 last_publish_time_ = ros::Time::now();
00812 first_publish_ = false;
00813 }
00814
00815
00816 void WG06::diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00817 {
00818 stringstream str;
00819 str << "Accelerometer (" << actuator_info_.name_ << ")";
00820 d.name = str.str();
00821 char serial[32];
00822 snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
00823 d.hardware_id = serial;
00824
00825 d.summary(d.OK, "OK");
00826 d.clear();
00827
00828 pr2_hardware_interface::AccelerometerCommand acmd(accelerometer_.command_);
00829
00830 const char * range_str =
00831 (acmd.range_ == 0) ? "+/-2G" :
00832 (acmd.range_ == 1) ? "+/-4G" :
00833 (acmd.range_ == 2) ? "+/-8G" :
00834 "INVALID";
00835
00836 const char * bandwidth_str =
00837 (acmd.bandwidth_ == 6) ? "1500Hz" :
00838 (acmd.bandwidth_ == 5) ? "750Hz" :
00839 (acmd.bandwidth_ == 4) ? "375Hz" :
00840 (acmd.bandwidth_ == 3) ? "190Hz" :
00841 (acmd.bandwidth_ == 2) ? "100Hz" :
00842 (acmd.bandwidth_ == 1) ? "50Hz" :
00843 (acmd.bandwidth_ == 0) ? "25Hz" :
00844 "INVALID";
00845
00846
00847 bool has_accelerometer = (board_major_ >= 2);
00848 double sample_frequency = 0.0;
00849 ros::Time current_time(ros::Time::now());
00850 if (!first_publish_)
00851 {
00852 sample_frequency = double(accelerometer_samples_) / (current_time - last_publish_time_).toSec();
00853 {
00854 if (((sample_frequency < 2000) || (sample_frequency > 4000)) && has_accelerometer)
00855 {
00856 d.mergeSummary(d.WARN, "Bad accelerometer sampling frequency");
00857 }
00858 }
00859 }
00860 accelerometer_samples_ = 0;
00861
00862 d.addf("Accelerometer", "%s", accelerometer_.state_.samples_.size() > 0 ? "Ok" : "Not Present");
00863 d.addf("Accelerometer range", "%s (%d)", range_str, acmd.range_);
00864 d.addf("Accelerometer bandwidth", "%s (%d)", bandwidth_str, acmd.bandwidth_);
00865 d.addf("Accelerometer sample frequency", "%f", sample_frequency);
00866 d.addf("Accelerometer missed samples", "%d", accelerometer_missed_samples_);
00867 }
00868
00869
00870 void WG06::diagnosticsWG06(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00871 {
00872 WG0X::diagnostics(d, buffer);
00873
00874 }
00875
00876 void WG06::diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00877 {
00878 int status_bytes =
00879 has_accel_and_ft_ ? sizeof(WG06StatusWithAccelAndFT) :
00880 accel_publisher_ ? sizeof(WG06StatusWithAccel) :
00881 sizeof(WG0XStatus);
00882 WG06Pressure *pressure = (WG06Pressure *)(buffer + command_size_ + status_bytes);
00883
00884 stringstream str;
00885 str << "Pressure sensors (" << actuator_info_.name_ << ")";
00886 d.name = str.str();
00887 char serial[32];
00888 snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
00889 d.hardware_id = serial;
00890 d.clear();
00891
00892 if (enable_pressure_sensor_)
00893 {
00894 d.summary(d.OK, "OK");
00895 }
00896 else
00897 {
00898 d.summary(d.OK, "Pressure sensor disabled by user");
00899 }
00900
00901 if (pressure_checksum_error_)
00902 {
00903 d.mergeSummary(d.ERROR, "Checksum error on pressure data");
00904 }
00905
00906 if (enable_pressure_sensor_)
00907 {
00908
00909 unsigned l_finger_good_count = 0;
00910 unsigned r_finger_good_count = 0;
00911
00912 for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
00913 {
00914 uint16_t l_data = pressure->l_finger_tip_[region_num];
00915 if ((l_data != 0xFFFF) && (l_data != 0x0000))
00916 {
00917 ++l_finger_good_count;
00918 }
00919
00920 uint16_t r_data = pressure->r_finger_tip_[region_num];
00921 if ((r_data != 0xFFFF) && (r_data != 0x0000))
00922 {
00923 ++r_finger_good_count;
00924 }
00925 }
00926
00927
00928
00929
00930 if ((l_finger_good_count == 0) && (r_finger_good_count == 0))
00931 {
00932 d.mergeSummary(d.WARN, "Pressure sensors may not be connected");
00933 }
00934 else
00935 {
00936
00937 if (l_finger_good_count == 0)
00938 {
00939 d.mergeSummary(d.WARN, "Sensor on left finger may not be connected");
00940 }
00941 else if (l_finger_good_count < NUM_PRESSURE_REGIONS)
00942 {
00943 d.mergeSummary(d.WARN, "Sensor on left finger may have damaged sensor regions");
00944 }
00945
00946 if (r_finger_good_count == 0)
00947 {
00948 d.mergeSummary(d.WARN, "Sensor on right finger may not be connected");
00949 }
00950 else if (r_finger_good_count < NUM_PRESSURE_REGIONS)
00951 {
00952 d.mergeSummary(d.WARN, "Sensor on right finger may have damaged sensor regions");
00953 }
00954 }
00955
00956 d.addf("Timestamp", "%u", pressure->timestamp_);
00957 d.addf("Data size", "%u", pressure_size_);
00958 d.addf("Checksum error count", "%u", pressure_checksum_error_count_);
00959
00960 {
00961 std::stringstream ss;
00962
00963 for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
00964 {
00965 ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
00966 << pressure->r_finger_tip_[region_num] << " ";
00967 if (region_num%8 == 7)
00968 ss << std::endl;
00969 }
00970 d.add("Right finger data", ss.str());
00971
00972 ss.str("");
00973
00974 for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
00975 {
00976 ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
00977 << pressure->l_finger_tip_[region_num] << " ";
00978 if (region_num%8 == 7) ss << std::endl;
00979 }
00980 d.add("Left finger data", ss.str());
00981 }
00982 }
00983
00984 }
00985
00986
00987 void WG06::diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status)
00988 {
00989 stringstream str;
00990 str << "Force/Torque sensor (" << actuator_info_.name_ << ")";
00991 d.name = str.str();
00992 char serial[32];
00993 snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
00994 d.hardware_id = serial;
00995
00996 d.summary(d.OK, "OK");
00997 d.clear();
00998
00999 ros::Time current_time(ros::Time::now());
01000 double sample_frequency = 0.0;
01001 if (!first_publish_)
01002 {
01003 sample_frequency = double(ft_sample_count_ - diag_last_ft_sample_count_) / (current_time - last_publish_time_).toSec();
01004 }
01005 diag_last_ft_sample_count_ = ft_sample_count_;
01006
01007
01008 d.addf("F/T sample frequency", "%.2f (Hz)", sample_frequency);
01009 d.addf("F/T missed samples", "%llu", ft_missed_samples_);
01010 std::stringstream ss;
01011 const FTDataSample &sample(status->ft_samples_[0]);
01012 for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
01013 {
01014 ss.str(""); ss << "Ch"<< (i);
01015 d.addf(ss.str(), "%d", int(sample.data_[i]));
01016 }
01017 d.addf("FT Vhalf", "%d", int(sample.vhalf_));
01018
01019 if (ft_overload_flags_ != 0)
01020 {
01021 d.mergeSummary(d.ERROR, "Sensor overloaded");
01022 ss.str("");
01023 for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
01024 {
01025 ss << "Ch" << i << " ";
01026 }
01027 }
01028 else
01029 {
01030 ss.str("None");
01031 }
01032 d.add("Overload Channels", ss.str());
01033
01034 if (ft_sampling_rate_error_)
01035 {
01036 d.mergeSummary(d.ERROR, "Sampling rate error");
01037 }
01038
01039 if (ft_disconnected_)
01040 {
01041 d.mergeSummary(d.ERROR, "Amplifier disconnected");
01042 }
01043 else if (ft_vhalf_error_)
01044 {
01045 d.mergeSummary(d.ERROR, "Vhalf error, amplifier circuity may be damaged");
01046 }
01047
01048 const std::vector<double> &ft_data( ft_analog_in_.state_.state_ );
01049 if (ft_data.size() == 6)
01050 {
01051 d.addf("Force X", "%f", ft_data[0]);
01052 d.addf("Force Y", "%f", ft_data[1]);
01053 d.addf("Force Z", "%f", ft_data[2]);
01054 d.addf("Torque X", "%f", ft_data[3]);
01055 d.addf("Torque Y", "%f", ft_data[4]);
01056 d.addf("Torque Z", "%f", ft_data[5]);
01057 }
01058 }
01059
01060
01061
01062 FTParamsInternal::FTParamsInternal()
01063 {
01064
01065
01066
01067 for (int i=0; i<6; ++i)
01068 {
01069 offset(i) = 0.0;
01070 gain(i) = 1.0;
01071 for (int j=0; j<6; ++j)
01072 {
01073 calibration_coeff(i,j) = (i==j) ? 1.0 : 0.0;
01074 }
01075 }
01076 }
01077
01078
01079 void FTParamsInternal::print() const
01080 {
01081 for (int i=0; i<6; ++i)
01082 {
01083 ROS_INFO("offset[%d] = %f", i, offset(i));
01084 }
01085 for (int i=0; i<6; ++i)
01086 {
01087 ROS_INFO("gain[%d] = %f", i, gain(i));
01088 }
01089 for (int i=0; i<6; ++i)
01090 {
01091 ROS_INFO("coeff[%d] = [%f,%f,%f,%f,%f,%f]", i,
01092 calibration_coeff(i,0), calibration_coeff(i,1),
01093 calibration_coeff(i,2), calibration_coeff(i,3),
01094 calibration_coeff(i,4), calibration_coeff(i,5)
01095 );
01096 }
01097 }
01098
01099
01100 bool FTParamsInternal::getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len)
01101 {
01102 if(!params.hasMember(name))
01103 {
01104 ROS_ERROR("Expected ft_param to have '%s' element", name);
01105 return false;
01106 }
01107
01108 XmlRpc::XmlRpcValue values = params[name];
01109 if (values.getType() != XmlRpc::XmlRpcValue::TypeArray)
01110 {
01111 ROS_ERROR("Expected FT param '%s' to be list type", name);
01112 return false;
01113 }
01114 if (values.size() != int(len))
01115 {
01116 ROS_ERROR("Expected FT param '%s' to have %d elements", name, len);
01117 return false;
01118 }
01119 for (unsigned i=0; i<len; ++i)
01120 {
01121 if (values[i].getType() != XmlRpc::XmlRpcValue::TypeDouble)
01122 {
01123 ROS_ERROR("Expected FT param %s[%d] to be floating point type", name, i);
01124 return false;
01125 } else {
01126 results[i] = static_cast<double>(values[i]);
01127 }
01128 }
01129
01130 return true;
01131 }
01132
01133
01143 bool FTParamsInternal::getRosParams(ros::NodeHandle nh)
01144 {
01145 if (!nh.hasParam("ft_params"))
01146 {
01147 ROS_WARN("'ft_params' not available for force/torque sensor in namespace '%s'",
01148 nh.getNamespace().c_str());
01149 return false;
01150 }
01151
01152 XmlRpc::XmlRpcValue params;
01153 nh.getParam("ft_params", params);
01154 if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
01155 {
01156 ROS_ERROR("expected ft_params to be struct type");
01157 return false;
01158 }
01159
01160 if (!getDoubleArray(params, "offsets", offsets_, 6))
01161 {
01162 return false;
01163 }
01164
01165 if (!getDoubleArray(params, "gains", gains_, 6))
01166 {
01167 return false;
01168 }
01169
01170 XmlRpc::XmlRpcValue coeff_matrix = params["calibration_coeff"];
01171 if (coeff_matrix.getType() != XmlRpc::XmlRpcValue::TypeArray)
01172 {
01173 ROS_ERROR("Expected FT param 'calibration_coeff' to be list type");
01174 return false;
01175 }
01176 if (coeff_matrix.size() != 6)
01177 {
01178 ROS_ERROR("Expected FT param 'calibration_coeff' to have 6 elements");
01179 return false;
01180 }
01181
01182 for (int i=0; i<6; ++i)
01183 {
01184 XmlRpc::XmlRpcValue coeff_row = coeff_matrix[i];
01185 if (coeff_row.getType() != XmlRpc::XmlRpcValue::TypeArray)
01186 {
01187 ROS_ERROR("Expected FT param calibration_coeff[%d] to be list type", i);
01188 return false;
01189 }
01190 if (coeff_row.size() != 6)
01191 {
01192 ROS_ERROR("Expected FT param calibration_coeff[%d] to have 6 elements", i);
01193 return false;
01194 }
01195
01196 for (int j=0; j<6; ++j)
01197 {
01198 if (coeff_row[j].getType() != XmlRpc::XmlRpcValue::TypeDouble)
01199 {
01200 ROS_ERROR("Expected FT param calibration_coeff[%d,%d] to be floating point type", i,j);
01201 return false;
01202 } else {
01203 calibration_coeff(i,j) = static_cast<double>(coeff_row[j]);
01204 }
01205 }
01206 }
01207
01208 return true;
01209 }
01210
01211
01212 const unsigned WG06::NUM_FT_CHANNELS;
01213 const unsigned WG06::MAX_FT_SAMPLES;