wg06.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
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_EXPORT_CLASS(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   // ft_publisher_(NULL)
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   // As good a place as any for making sure that compiler actually packed these structures correctly
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     // Do nothing - status memory map is same size as WG05
00111   }
00112   if (fw_major_ == 1)
00113   {
00114     // Include Accelerometer data
00115     status_size_ = base_status_size = sizeof(WG06StatusWithAccel);
00116   }
00117   else if ((fw_major_ == 2) || (fw_major_ == 3))
00118   {
00119     // Include Accelerometer and Force/Torque sensor data
00120     status_size_ = base_status_size = sizeof(WG06StatusWithAccelAndFT);
00121     has_accel_and_ft_ = true;
00122     if (fw_major_ == 3)
00123     {
00124       // Pressure data size is 513 bytes instead of 94.  The physical address has also moved.
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   //ROS_DEBUG("device %d, command  0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
00138   (*fmmu)[0] = EC_FMMU(start_address, // Logical start address
00139                        command_size_,// Logical length
00140                        0x00, // Logical StartBit
00141                        0x07, // Logical EndBit
00142                        COMMAND_PHY_ADDR, // Physical Start address
00143                        0x00, // Physical StartBit
00144                        false, // Read Enable
00145                        true, // Write Enable
00146                        true); // Enable
00147 
00148   start_address += command_size_;
00149 
00150   //ROS_DEBUG("device %d, status   0x%X = 0x10000+%d", (int)sh->get_ring_position(), start_address, start_address-0x10000);
00151   (*fmmu)[1] = EC_FMMU(start_address, // Logical start address
00152                        base_status_size, // Logical length
00153                        0x00, // Logical StartBit
00154                        0x07, // Logical EndBit
00155                        STATUS_PHY_ADDR, // Physical Start address
00156                        0x00, // Physical StartBit
00157                        true, // Read Enable
00158                        false, // Write Enable
00159                        true); // Enable
00160 
00161   start_address += base_status_size;
00162 
00163   (*fmmu)[2] = EC_FMMU(start_address, // Logical start address
00164                        pressure_size_, // Logical length
00165                        0x00, // Logical StartBit
00166                        0x07, // Logical EndBit
00167                        pressure_phy_addr, // Physical Start address
00168                        0x00, // Physical StartBit
00169                        true, // Read Enable
00170                        false, // Write Enable
00171                        true); // Enable
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   // Sync managers
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); // wait for ros to flush rosconsole output
00219       return -1;
00220     }
00221 
00222     // For some versions of software pressure and force/torque sensors can be
00223     // selectively enabled / disabled    
00224     ros::NodeHandle nh(string("~/") + actuator_.name_);
00225     if (!nh.getParam("enable_pressure_sensor", enable_pressure_sensor_))
00226     {
00227       enable_pressure_sensor_ = true; //default to to true
00228     }
00229     if (!nh.getParam("enable_ft_sensor", enable_ft_sensor_))
00230     {
00231       enable_ft_sensor_ = false; //default to to 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     // FW version 2+ supports selectively enabling/disabling pressure and F/T sensor
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     // Publish accelerometer data as a ROS topic, if firmware is recent enough
00263     if (fw_major_ >= 1)
00264     {
00265       if (!initializeAccel(hw))
00266       {
00267         return -1;
00268       }
00269     }
00270 
00271     // FW version 2 supports Force/Torque sensor.
00272     // Provide Force/Torque data to controllers as an AnalogIn vector 
00273     if ((fw_major_ >= 2) && (enable_ft_sensor_))
00274     {
00275       if (!initializeFT(hw))
00276       {
00277         return -1;
00278       }
00279     }
00280 
00281     // FW version 2 and 3 uses soft-processors to control certain peripherals.
00282     // Allow the firmware on these soft-processors to be read/write through ROS service calls
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   // Publish pressure sensor data as a ROS topic
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   // Register pressure sensor with pr2_hardware_interface::HardwareInterface
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   // Register accelerometer with pr2_hardware_interface::HardwareInterface
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 provides 6 values : 3 Forces + 3 Torques
00353   ft_raw_analog_in_.state_.state_.resize(6); 
00354   // FT usually provides 3-4 new samples per cycle
00355   force_torque_.state_.samples_.reserve(4);
00356   force_torque_.state_.good_ = true;
00357 
00358   // For now publish RAW F/T values for engineering purposes.  In future this publisher may be disabled by default.
00359   std::string topic = "raw_ft";
00360   if (!actuator_.name_.empty())
00361     topic = topic + "/" + string(actuator_.name_);
00362   raw_ft_publisher_ = new realtime_tools::RealtimePublisher<ethercat_hardware::RawFTData>(ros::NodeHandle(), topic, 1);
00363   if (raw_ft_publisher_ == NULL)
00364   {
00365     ROS_FATAL("Could not allocate raw_ft publisher");
00366     return false;
00367   }
00368   // Allocate space for raw f/t data values
00369   raw_ft_publisher_->msg_.samples.reserve(MAX_FT_SAMPLES);
00370 
00371   force_torque_.command_.halt_on_error_ = false;
00372   force_torque_.state_.good_ = true;
00373 
00374   if (!actuator_.name_.empty())
00375   {
00376     ft_analog_in_.state_.state_.resize(6);
00377     ros::NodeHandle nh("~" + string(actuator_.name_));
00378     FTParamsInternal ft_params;
00379     if ( ft_params.getRosParams(nh) )
00380     {
00381       ft_params_ = ft_params;
00382       ft_params_.print();
00383       // If we have ft_params, publish F/T values.  
00384       topic = "ft";
00385       if (!actuator_.name_.empty())
00386         topic = topic + "/" + string(actuator_.name_);
00387       ft_publisher_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(ros::NodeHandle(), topic, 1);
00388       if (ft_publisher_ == NULL)
00389       {
00390         ROS_FATAL("Could not allocate ft publisher");
00391         return false;
00392       }
00393 
00394       // Register force/torque sensor with pr2_hardware_interface::HardwareInterface
00395       force_torque_.name_ = actuator_.name_;
00396       if (hw && !hw->addForceTorque(&force_torque_))
00397       {
00398         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());
00399         return false;
00400       }
00401     }
00402   }
00403 
00404   return true;
00405 }
00406 
00407 
00408 bool WG06::initializeSoftProcessor()
00409 {
00410   // TODO: do not use direct access to device.  Not safe while realtime loop is running
00411   // ALSO, this leaks memory
00412   EthercatDirectCom *com = new EthercatDirectCom(EtherCAT_DataLinkLayer::instance());
00413 
00414   // Add soft-processors to list
00415   soft_processor_.add(&mailbox_, actuator_.name_, "pressure", 0xA000, 0x249);
00416   soft_processor_.add(&mailbox_, actuator_.name_, "accel", 0xB000, 0x24A);
00417 
00418   // Start services
00419   if (!soft_processor_.initialize(com))
00420   {
00421     return false;
00422   }
00423 
00424   return true;
00425 }
00426 
00427 
00428 
00429 void WG06::packCommand(unsigned char *buffer, bool halt, bool reset)
00430 {
00431   if (reset) 
00432   {
00433     pressure_checksum_error_ = false;
00434     ft_overload_flags_ = 0;
00435     ft_disconnected_ = false;
00436     ft_vhalf_error_ = false;
00437     ft_sampling_rate_error_ = false;
00438   }
00439 
00440   WG0X::packCommand(buffer, halt, reset);
00441 
00442   WG0XCommand *c = (WG0XCommand *)buffer;
00443 
00444   if (accelerometer_.command_.range_ > 2 || 
00445       accelerometer_.command_.range_ < 0)
00446     accelerometer_.command_.range_ = 0;
00447 
00448   if (accelerometer_.command_.bandwidth_ > 6 || 
00449       accelerometer_.command_.bandwidth_ < 0)
00450     accelerometer_.command_.bandwidth_ = 0;
00451   
00452   c->digital_out_ = (digital_out_.command_.data_ != 0) |
00453     ((accelerometer_.command_.bandwidth_ & 0x7) << 1) | 
00454     ((accelerometer_.command_.range_ & 0x3) << 4); 
00455   c->checksum_ = wg_util::rotateRight8(wg_util::computeChecksum(c, command_size_ - 1));
00456 }
00457 
00458 
00459 bool WG06::unpackState(unsigned char *this_buffer, unsigned char *prev_buffer)
00460 {
00461   bool rv = true;
00462 
00463   int status_bytes = 
00464     has_accel_and_ft_  ? sizeof(WG06StatusWithAccelAndFT) :  // Has FT sensor and accelerometer
00465     accel_publisher_   ? sizeof(WG06StatusWithAccel) : 
00466                          sizeof(WG0XStatus);  
00467 
00468   unsigned char *pressure_buf = (this_buffer + command_size_ + status_bytes);
00469 
00470   unsigned char* this_status = this_buffer + command_size_;
00471   if (!verifyChecksum(this_status, status_bytes))
00472   {
00473     status_checksum_error_ = true;
00474     rv = false;
00475     goto end;
00476   }
00477 
00478   if (!unpackPressure(pressure_buf))
00479   {
00480     rv = false;
00481     //goto end;  // all other tasks should not be effected by bad pressure sensor data
00482   }
00483 
00484   if (accel_publisher_)
00485   {
00486     WG06StatusWithAccel *status = (WG06StatusWithAccel *)(this_buffer + command_size_);
00487     WG06StatusWithAccel *last_status = (WG06StatusWithAccel *)(prev_buffer + command_size_);
00488     if (!unpackAccel(status, last_status))
00489     {
00490       rv=false;
00491     }
00492   }
00493 
00494   if (has_accel_and_ft_ && enable_ft_sensor_)
00495   {
00496     WG06StatusWithAccelAndFT *status = (WG06StatusWithAccelAndFT *)(this_buffer + command_size_);
00497     WG06StatusWithAccelAndFT *last_status = (WG06StatusWithAccelAndFT *)(prev_buffer + command_size_);
00498     if (!unpackFT(status, last_status))
00499     {
00500       rv = false;
00501     }
00502   }
00503 
00504 
00505   if (!WG0X::unpackState(this_buffer, prev_buffer))
00506   {
00507     rv = false;
00508   }
00509 
00510  end:
00511   return rv;
00512 }
00513 
00514 
00520 bool WG06::unpackPressure(unsigned char *pressure_buf)
00521 {  
00522   if (!enable_pressure_sensor_)
00523   {
00524     // If pressure sensor is not enabled don't attempt to do anything with pressure data
00525     return true;
00526   }
00527 
00528   if (!verifyChecksum(pressure_buf, pressure_size_))
00529   {
00530     ++pressure_checksum_error_count_;
00531     if (false /* debugging */)
00532     {
00533       std::stringstream ss;
00534       ss << "Pressure buffer checksum error : " << std::endl;
00535       for (unsigned ii=0; ii<pressure_size_; ++ii)
00536       {
00537         ss << std::uppercase << std::hex << std::setw(2) << std::setfill('0')
00538            << unsigned(pressure_buf[ii]) << " ";
00539         if ((ii%8) == 7) ss << std::endl;
00540       }
00541       ROS_ERROR_STREAM(ss.str());
00542       std::cerr << ss.str() << std::endl;
00543     }
00544     pressure_checksum_error_ = true;
00545     return false;
00546   }
00547   else 
00548   {
00549     WG06Pressure *p( (WG06Pressure *) pressure_buf);
00550     for (int i = 0; i < 22; ++i ) {
00551       pressure_sensors_[0].state_.data_[i] =
00552         ((p->l_finger_tip_[i] >> 8) & 0xff) |
00553         ((p->l_finger_tip_[i] << 8) & 0xff00);
00554       pressure_sensors_[1].state_.data_[i] =
00555         ((p->r_finger_tip_[i] >> 8) & 0xff) |
00556         ((p->r_finger_tip_[i] << 8) & 0xff00);
00557     }
00558 
00559     if (p->timestamp_ != last_pressure_time_)
00560     {
00561       if (pressure_publisher_ && pressure_publisher_->trylock())
00562       {
00563         pressure_publisher_->msg_.header.stamp = ros::Time::now();
00564         pressure_publisher_->msg_.l_finger_tip.resize(22);
00565         pressure_publisher_->msg_.r_finger_tip.resize(22);
00566         for (int i = 0; i < 22; ++i ) {
00567           pressure_publisher_->msg_.l_finger_tip[i] = pressure_sensors_[0].state_.data_[i];
00568           pressure_publisher_->msg_.r_finger_tip[i] = pressure_sensors_[1].state_.data_[i];
00569         }
00570         pressure_publisher_->unlockAndPublish();
00571       }
00572     }
00573     last_pressure_time_ = p->timestamp_;
00574   }
00575 
00576   return true;
00577 }
00578 
00579 
00585 bool WG06::unpackAccel(WG06StatusWithAccel *status, WG06StatusWithAccel *last_status)
00586 {
00587   int count = uint8_t(status->accel_count_ - last_status->accel_count_);
00588   accelerometer_samples_ += count;
00589   // Only most recent 4 samples of accelerometer data is available in status data
00590   // 4 samples will be enough with realtime loop running at 1kHz and accelerometer running at 3kHz
00591   // If count is greater than 4, then some data has been "missed".
00592   accelerometer_missed_samples_ += (count > 4) ? (count-4) : 0; 
00593   count = min(4, count);
00594   accelerometer_.state_.samples_.resize(count);
00595   accelerometer_.state_.frame_id_ = string(actuator_info_.name_) + "_accelerometer_link";
00596   for (int i = 0; i < count; ++i)
00597   {
00598     int32_t acc = status->accel_[count - i - 1];
00599     int range = (acc >> 30) & 3;
00600     float d = 1 << (8 - range);
00601     accelerometer_.state_.samples_[i].x = 9.81 * ((((acc >>  0) & 0x3ff) << 22) >> 22) / d;
00602     accelerometer_.state_.samples_[i].y = 9.81 * ((((acc >> 10) & 0x3ff) << 22) >> 22) / d;
00603     accelerometer_.state_.samples_[i].z = 9.81 * ((((acc >> 20) & 0x3ff) << 22) >> 22) / d;
00604   }
00605 
00606   if (accel_publisher_->trylock())
00607   {
00608     accel_publisher_->msg_.header.frame_id = accelerometer_.state_.frame_id_;
00609     accel_publisher_->msg_.header.stamp = ros::Time::now();
00610     accel_publisher_->msg_.samples.resize(count);
00611     for (int i = 0; i < count; ++i)
00612     {
00613       accel_publisher_->msg_.samples[i].x = accelerometer_.state_.samples_[i].x;
00614       accel_publisher_->msg_.samples[i].y = accelerometer_.state_.samples_[i].y;
00615       accel_publisher_->msg_.samples[i].z = accelerometer_.state_.samples_[i].z;
00616     }
00617     accel_publisher_->unlockAndPublish();
00618   }
00619   return true;
00620 }
00621 
00622 
00644 void WG06::convertFTDataSampleToWrench(const FTDataSample &sample, geometry_msgs::Wrench &wrench)
00645 {
00646   // Apply gains / offset to F/T raw analog inputs
00647   // Also, make sure values are within bounds.  
00648   // Out-of-bound values indicate a broken sensor or an overload.
00649   double in[6];
00650   for (unsigned i=0; i<6; ++i)
00651   {
00652     int raw_data = sample.data_[i];
00653     if (abs(raw_data) > ft_overload_limit_)
00654     {
00655       ft_overload_flags_ |= (1<<i);
00656     }
00657     in[i] = (double(raw_data) - ft_params_.offset(i)) / ( ft_params_.gain(i) * double(1<<16) );
00658   }
00659 
00660   // Vhalf ADC measurement should be amost half the ADC reference voltage
00661   // For a 16bit ADC the Vhalf value should be about (1<<16)/2
00662   // If Vhalf value is not close to this, this could mean 1 of 2 things:
00663   //   1. WG035 electronics are damaged some-how 
00664   //   2. WG035 is not present or disconnected gripper MCB 
00665   if ( abs( int(sample.vhalf_) - FT_VHALF_IDEAL) > FT_VHALF_RANGE )
00666   {
00667     if ((sample.vhalf_ == 0x0000) || (sample.vhalf_ == 0xFFFF))
00668     {
00669       // When WG035 MCB is not present the DATA line floats low or high and all 
00670       // reads through SPI interface return 0.
00671       ft_disconnected_ = true;
00672     }
00673     else 
00674     {
00675       ft_vhalf_error_ = true;
00676     }
00677   }
00678 
00679   // Apply coeffiecient matrix multiplication to raw inputs to get force/torque values
00680   double out[6];
00681   for (unsigned i=0; i<6; ++i)
00682   {
00683     double sum=0.0;
00684     for (unsigned j=0; j<6; ++j)
00685     {
00686       sum += ft_params_.calibration_coeff(i,j) * in[j];
00687     }
00688     out[i] = sum;
00689   }
00690 
00691   wrench.force.x  = out[0];
00692   wrench.force.y  = out[1];
00693   wrench.force.z  = out[2];
00694   wrench.torque.x = out[3];
00695   wrench.torque.y = out[4];
00696   wrench.torque.z = out[5];
00697 }
00698 
00699 
00705 bool WG06::unpackFT(WG06StatusWithAccelAndFT *status, WG06StatusWithAccelAndFT *last_status)
00706 {  
00707   pr2_hardware_interface::ForceTorqueState &ft_state(force_torque_.state_);
00708 
00709   ros::Time current_time(ros::Time::now());
00710 
00711   // Fill in raw analog output with most recent data sample, (might become deprecated?)
00712   {
00713     ft_raw_analog_in_.state_.state_.resize(6);
00714     const FTDataSample &sample(status->ft_samples_[0]);
00715     for (unsigned i=0; i<6; ++i)
00716     {
00717       int raw_data = sample.data_[i];
00718       ft_raw_analog_in_.state_.state_[i] = double(raw_data);
00719     }
00720   }
00721 
00722   unsigned new_samples = (unsigned(status->ft_sample_count_) - unsigned(last_status->ft_sample_count_)) & 0xFF;
00723   ft_sample_count_ += new_samples;
00724   int missed_samples = std::max(int(0), int(new_samples) - 4);
00725   ft_missed_samples_ += missed_samples;
00726   unsigned usable_samples = min(new_samples, MAX_FT_SAMPLES); 
00727 
00728   // Also, if number of new_samples is ever 0, then there is also an error
00729   if (usable_samples == 0)
00730   {
00731     ft_sampling_rate_error_ = true;
00732   }
00733 
00734   // Make room in data structure for more f/t samples
00735   ft_state.samples_.resize(usable_samples);
00736 
00737   // If any f/t channel is overload or the sampling rate is bad, there is an error.
00738   ft_state.good_ = ( (!ft_sampling_rate_error_) && 
00739                      (ft_overload_flags_ == 0) && 
00740                      (!ft_disconnected_) && 
00741                      (!ft_vhalf_error_) );
00742 
00743   for (unsigned sample_index=0; sample_index<usable_samples; ++sample_index)
00744   {
00745     // samples are stored in status data, so that newest sample is at index 0.
00746     // this is the reverse of the order data is stored in hardware_interface::ForceTorque buffer.
00747     unsigned status_sample_index = usable_samples-sample_index-1;
00748     const FTDataSample &sample(status->ft_samples_[status_sample_index]); 
00749     geometry_msgs::Wrench &wrench(ft_state.samples_[sample_index]);
00750     convertFTDataSampleToWrench(sample, wrench);
00751   }
00752 
00753   // Put newest sample into analog vector for controllers (deprecated)
00754   if (usable_samples > 0)
00755   {
00756     const geometry_msgs::Wrench &wrench(ft_state.samples_[usable_samples-1]);
00757     ft_analog_in_.state_.state_[0] = wrench.force.x;
00758     ft_analog_in_.state_.state_[1] = wrench.force.y;
00759     ft_analog_in_.state_.state_[2] = wrench.force.z;
00760     ft_analog_in_.state_.state_[3] = wrench.torque.x;
00761     ft_analog_in_.state_.state_[4] = wrench.torque.y;
00762     ft_analog_in_.state_.state_[5] = wrench.torque.z;
00763   }
00764 
00765   // Put all new samples in buffer and publish it
00766   if ((raw_ft_publisher_ != NULL) && (raw_ft_publisher_->trylock()))
00767   {
00768     raw_ft_publisher_->msg_.samples.resize(usable_samples);
00769     raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
00770     raw_ft_publisher_->msg_.missed_samples = ft_missed_samples_;
00771     for (unsigned sample_num=0; sample_num<usable_samples; ++sample_num)
00772     {
00773       // put data into message so oldest data is first element
00774       const FTDataSample &sample(status->ft_samples_[sample_num]);
00775       ethercat_hardware::RawFTDataSample &msg_sample(raw_ft_publisher_->msg_.samples[usable_samples-sample_num-1]);
00776       msg_sample.sample_count = ft_sample_count_ - sample_num;
00777       msg_sample.data.resize(NUM_FT_CHANNELS);
00778       for (unsigned ch_num=0; ch_num<NUM_FT_CHANNELS; ++ch_num)
00779       {
00780         msg_sample.data[ch_num] = sample.data_[ch_num];
00781       }
00782       msg_sample.vhalf = sample.vhalf_;
00783     }
00784     raw_ft_publisher_->msg_.sample_count = ft_sample_count_;
00785     raw_ft_publisher_->unlockAndPublish();
00786   }
00787 
00788   // Put newest sample in realtime publisher
00789   if ( (usable_samples > 0) && (ft_publisher_ != NULL) && (ft_publisher_->trylock()) )
00790   {
00791     ft_publisher_->msg_.header.stamp = current_time;
00792     ft_publisher_->msg_.wrench = ft_state.samples_[usable_samples-1];
00793     ft_publisher_->unlockAndPublish();
00794   }
00795 
00796   // If this returns false, it will cause motors to halt.
00797   // Return "good_" state of sensor, unless halt_on_error is false. 
00798   return ft_state.good_ || !force_torque_.command_.halt_on_error_;
00799 }
00800 
00801 
00802 void WG06::multiDiagnostics(vector<diagnostic_msgs::DiagnosticStatus> &vec, unsigned char *buffer)
00803 {
00804   diagnostic_updater::DiagnosticStatusWrapper &d(diagnostic_status_);
00805   diagnosticsWG06(d, buffer);  
00806   vec.push_back(d);
00807   diagnosticsAccel(d, buffer);
00808   vec.push_back(d);  
00809   diagnosticsPressure(d, buffer);
00810   vec.push_back(d);
00811 
00812   if (has_accel_and_ft_ && enable_ft_sensor_)
00813   {
00814     WG06StatusWithAccelAndFT *status = (WG06StatusWithAccelAndFT *)(buffer + command_size_);
00815     // perform f/t sample
00816     diagnosticsFT(d, status);
00817     vec.push_back(d);
00818   }
00819 
00820   last_publish_time_ = ros::Time::now();
00821   first_publish_ = false;
00822 }
00823 
00824 
00825 void WG06::diagnosticsAccel(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00826 {
00827   stringstream str;
00828   str << "Accelerometer (" << actuator_info_.name_ << ")";
00829   d.name = str.str();
00830   char serial[32];
00831   snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
00832   d.hardware_id = serial;
00833 
00834   d.summary(d.OK, "OK");
00835   d.clear();
00836   
00837   pr2_hardware_interface::AccelerometerCommand acmd(accelerometer_.command_);
00838 
00839   const char * range_str = 
00840     (acmd.range_ == 0) ? "+/-2G" :
00841     (acmd.range_ == 1) ? "+/-4G" :
00842     (acmd.range_ == 2) ? "+/-8G" :
00843     "INVALID";
00844 
00845   const char * bandwidth_str = 
00846     (acmd.bandwidth_ == 6) ? "1500Hz" :
00847     (acmd.bandwidth_ == 5)  ? "750Hz" :
00848     (acmd.bandwidth_ == 4)  ? "375Hz" :
00849     (acmd.bandwidth_ == 3)  ? "190Hz" :
00850     (acmd.bandwidth_ == 2)  ? "100Hz" :
00851     (acmd.bandwidth_ == 1)   ? "50Hz" :
00852     (acmd.bandwidth_ == 0)   ? "25Hz" :
00853     "INVALID";
00854 
00855   // Board revB=1 and revA=0 does not have accelerometer
00856   bool has_accelerometer = (board_major_ >= 2);
00857   double sample_frequency = 0.0;
00858   ros::Time current_time(ros::Time::now());
00859   if (!first_publish_)
00860   {
00861     sample_frequency = double(accelerometer_samples_) / (current_time - last_publish_time_).toSec();
00862     {
00863       if (((sample_frequency < 2000) || (sample_frequency > 4000)) && has_accelerometer)
00864       {
00865         d.mergeSummary(d.WARN, "Bad accelerometer sampling frequency");
00866       }
00867     }
00868   }
00869   accelerometer_samples_ = 0;
00870 
00871   d.addf("Accelerometer", "%s", accelerometer_.state_.samples_.size() > 0 ? "Ok" : "Not Present");
00872   d.addf("Accelerometer range", "%s (%d)", range_str, acmd.range_);
00873   d.addf("Accelerometer bandwidth", "%s (%d)", bandwidth_str, acmd.bandwidth_);
00874   d.addf("Accelerometer sample frequency", "%f", sample_frequency);
00875   d.addf("Accelerometer missed samples", "%d", accelerometer_missed_samples_);                                   
00876 }
00877 
00878 
00879 void WG06::diagnosticsWG06(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00880 {
00881   WG0X::diagnostics(d, buffer);
00882   // nothing else to do here 
00883 }
00884 
00885 void WG06::diagnosticsPressure(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer)
00886 {
00887   int status_bytes = 
00888     has_accel_and_ft_  ? sizeof(WG06StatusWithAccelAndFT) :  // Has FT sensor and accelerometer
00889     accel_publisher_   ? sizeof(WG06StatusWithAccel) : 
00890                          sizeof(WG0XStatus);
00891   WG06Pressure *pressure = (WG06Pressure *)(buffer + command_size_ + status_bytes);
00892 
00893   stringstream str;
00894   str << "Pressure sensors (" << actuator_info_.name_ << ")";
00895   d.name = str.str();
00896   char serial[32];
00897   snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
00898   d.hardware_id = serial;
00899   d.clear();
00900 
00901   if (enable_pressure_sensor_)
00902   {    
00903     d.summary(d.OK, "OK");
00904   }
00905   else
00906   {
00907     d.summary(d.OK, "Pressure sensor disabled by user");
00908   }
00909 
00910   if (pressure_checksum_error_)
00911   {
00912     d.mergeSummary(d.ERROR, "Checksum error on pressure data");
00913   }
00914     
00915   if (enable_pressure_sensor_)
00916   {
00917     // look at pressure sensor value to detect any damaged cables, or possibly missing sensor
00918     unsigned l_finger_good_count = 0;
00919     unsigned r_finger_good_count = 0;
00920 
00921     for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
00922     {
00923       uint16_t l_data = pressure->l_finger_tip_[region_num];
00924       if ((l_data != 0xFFFF) && (l_data != 0x0000))
00925       {
00926         ++l_finger_good_count;
00927       }
00928 
00929       uint16_t r_data = pressure->r_finger_tip_[region_num];
00930       if ((r_data != 0xFFFF) && (r_data != 0x0000))
00931       {
00932         ++r_finger_good_count;
00933       }
00934     }
00935       
00936     // if no pressure sensor regions are return acceptable data, then its possible that the
00937     // pressure sensor is not connected at all.  This is just a warning, because on many robots
00938     // the pressure sensor may not be installed
00939     if ((l_finger_good_count == 0) && (r_finger_good_count == 0))
00940     {
00941       d.mergeSummary(d.WARN, "Pressure sensors may not be connected");
00942     }
00943     else 
00944     {
00945       // At least one pressure sensor seems to be present ...       
00946       if (l_finger_good_count == 0)
00947       {
00948         d.mergeSummary(d.WARN, "Sensor on left finger may not be connected");
00949       }
00950       else if (l_finger_good_count < NUM_PRESSURE_REGIONS)
00951       {
00952         d.mergeSummary(d.WARN, "Sensor on left finger may have damaged sensor regions");
00953       }
00954 
00955       if (r_finger_good_count == 0)
00956       {
00957         d.mergeSummary(d.WARN, "Sensor on right finger may not be connected");
00958       }
00959       else if (r_finger_good_count < NUM_PRESSURE_REGIONS)
00960       {
00961         d.mergeSummary(d.WARN, "Sensor on right finger may have damaged sensor regions");
00962       }
00963     } 
00964 
00965     d.addf("Timestamp", "%u", pressure->timestamp_);
00966     d.addf("Data size", "%u", pressure_size_);
00967     d.addf("Checksum error count", "%u", pressure_checksum_error_count_);
00968 
00969     { // put right and left finger data in dianostics
00970       std::stringstream ss;
00971 
00972       for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
00973       {
00974         ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0') 
00975            << pressure->r_finger_tip_[region_num] << " ";
00976         if (region_num%8 == 7) 
00977           ss << std::endl;      
00978       }
00979       d.add("Right finger data",  ss.str());
00980 
00981       ss.str("");
00982 
00983       for (unsigned region_num=0; region_num<NUM_PRESSURE_REGIONS; ++region_num)
00984       {
00985         ss << std::uppercase << std::hex << std::setw(4) << std::setfill('0') 
00986            << pressure->l_finger_tip_[region_num] << " ";
00987         if (region_num%8 == 7) ss << std::endl;      
00988       }
00989       d.add("Left finger data",  ss.str());
00990     }
00991   }
00992 
00993 }
00994 
00995 
00996 void WG06::diagnosticsFT(diagnostic_updater::DiagnosticStatusWrapper &d, WG06StatusWithAccelAndFT *status)
00997 {
00998   stringstream str;
00999   str << "Force/Torque sensor (" << actuator_info_.name_ << ")";
01000   d.name = str.str();
01001   char serial[32];
01002   snprintf(serial, sizeof(serial), "%d-%05d-%05d", config_info_.product_id_ / 100000 , config_info_.product_id_ % 100000, config_info_.device_serial_number_);
01003   d.hardware_id = serial;
01004 
01005   d.summary(d.OK, "OK");
01006   d.clear();
01007 
01008   ros::Time current_time(ros::Time::now());
01009   double sample_frequency = 0.0;
01010   if (!first_publish_)
01011   {
01012     sample_frequency = double(ft_sample_count_ - diag_last_ft_sample_count_) / (current_time - last_publish_time_).toSec();
01013   }
01014   diag_last_ft_sample_count_ = ft_sample_count_;
01015 
01016   //d.addf("F/T sample count", "%llu", ft_sample_count_);
01017   d.addf("F/T sample frequency", "%.2f (Hz)", sample_frequency);
01018   d.addf("F/T missed samples", "%llu", ft_missed_samples_);
01019   std::stringstream ss;
01020   const FTDataSample &sample(status->ft_samples_[0]);  //use newest data sample
01021   for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
01022   {
01023     ss.str(""); ss << "Ch"<< (i);
01024     d.addf(ss.str(), "%d", int(sample.data_[i]));
01025   }
01026   d.addf("FT Vhalf", "%d", int(sample.vhalf_));
01027 
01028   if (ft_overload_flags_ != 0)
01029   {
01030     d.mergeSummary(d.ERROR, "Sensor overloaded");
01031     ss.str("");
01032     for (unsigned i=0;i<NUM_FT_CHANNELS;++i)
01033     {
01034       ss << "Ch" << i << " ";
01035     }
01036   }
01037   else 
01038   {
01039     ss.str("None");
01040   }
01041   d.add("Overload Channels", ss.str());
01042 
01043   if (ft_sampling_rate_error_)
01044   {
01045     d.mergeSummary(d.ERROR, "Sampling rate error");
01046   }
01047 
01048   if (ft_disconnected_)
01049   {
01050     d.mergeSummary(d.ERROR, "Amplifier disconnected");
01051   }
01052   else if (ft_vhalf_error_)
01053   {
01054     d.mergeSummary(d.ERROR, "Vhalf error, amplifier circuity may be damaged"); 
01055   }
01056 
01057   const std::vector<double> &ft_data( ft_analog_in_.state_.state_ );
01058   if (ft_data.size() == 6)
01059   {
01060     d.addf("Force X",  "%f", ft_data[0]);
01061     d.addf("Force Y",  "%f", ft_data[1]);
01062     d.addf("Force Z",  "%f", ft_data[2]);
01063     d.addf("Torque X", "%f", ft_data[3]);
01064     d.addf("Torque Y", "%f", ft_data[4]);
01065     d.addf("Torque Z", "%f", ft_data[5]);
01066   }
01067 }
01068 
01069 
01070 
01071 FTParamsInternal::FTParamsInternal()
01072 {
01073   // Initial offset = 0.0
01074   // Gains = 1.0 
01075   // Calibration coeff = identity matrix
01076   for (int i=0; i<6; ++i)
01077   {
01078     offset(i) = 0.0;
01079     gain(i) = 1.0;
01080     for (int j=0; j<6; ++j)
01081     {
01082       calibration_coeff(i,j) = (i==j) ? 1.0 : 0.0;
01083     }
01084   }
01085 }
01086 
01087 
01088 void FTParamsInternal::print() const
01089 {
01090   for (int i=0; i<6; ++i)
01091   {
01092     ROS_INFO("offset[%d] = %f", i, offset(i));
01093   }
01094   for (int i=0; i<6; ++i)
01095   {
01096     ROS_INFO("gain[%d] = %f", i, gain(i));
01097   }
01098   for (int i=0; i<6; ++i)
01099   {
01100     ROS_INFO("coeff[%d] = [%f,%f,%f,%f,%f,%f]", i, 
01101              calibration_coeff(i,0), calibration_coeff(i,1), 
01102              calibration_coeff(i,2), calibration_coeff(i,3), 
01103              calibration_coeff(i,4), calibration_coeff(i,5)
01104              );
01105   }
01106 }
01107 
01108 
01109 bool FTParamsInternal::getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len)
01110 {
01111   if(!params.hasMember(name))
01112   {
01113     ROS_ERROR("Expected ft_param to have '%s' element", name);
01114     return false;
01115   }
01116 
01117   XmlRpc::XmlRpcValue values = params[name];
01118   if (values.getType() != XmlRpc::XmlRpcValue::TypeArray)
01119   {
01120     ROS_ERROR("Expected FT param '%s' to be list type", name);
01121     return false;
01122   }
01123   if (values.size() != int(len))
01124   {
01125     ROS_ERROR("Expected FT param '%s' to have %d elements", name, len);
01126     return false;
01127   }
01128   for (unsigned i=0; i<len; ++i)
01129   {
01130     if (values[i].getType() != XmlRpc::XmlRpcValue::TypeDouble)
01131     {
01132       ROS_ERROR("Expected FT param %s[%d] to be floating point type", name, i);
01133       return false;
01134     } else {
01135       results[i] = static_cast<double>(values[i]);
01136     }
01137   }
01138 
01139   return true;
01140 }
01141 
01142 
01152 bool FTParamsInternal::getRosParams(ros::NodeHandle nh)
01153 {
01154   if (!nh.hasParam("ft_params"))
01155   {
01156     ROS_WARN("'ft_params' not available for force/torque sensor in namespace '%s'",
01157              nh.getNamespace().c_str());
01158     return false;
01159   }
01160 
01161   XmlRpc::XmlRpcValue params;
01162   nh.getParam("ft_params", params);
01163   if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct)
01164   {
01165     ROS_ERROR("expected ft_params to be struct type");
01166     return false;
01167   }
01168 
01169   if (!getDoubleArray(params, "offsets", offsets_, 6))
01170   {
01171     return false;
01172   }
01173 
01174   if (!getDoubleArray(params, "gains", gains_, 6))
01175   {
01176     return false;
01177   }
01178 
01179   XmlRpc::XmlRpcValue coeff_matrix = params["calibration_coeff"];
01180   if (coeff_matrix.getType() != XmlRpc::XmlRpcValue::TypeArray)
01181   {
01182     ROS_ERROR("Expected FT param 'calibration_coeff' to be list type");
01183     return false;
01184   }
01185   if (coeff_matrix.size() != 6)
01186   {
01187     ROS_ERROR("Expected FT param 'calibration_coeff' to have 6 elements");
01188     return false;
01189   }
01190 
01191   for (int i=0; i<6; ++i)
01192   {
01193     XmlRpc::XmlRpcValue coeff_row = coeff_matrix[i];
01194     if (coeff_row.getType() != XmlRpc::XmlRpcValue::TypeArray)
01195     {
01196       ROS_ERROR("Expected FT param calibration_coeff[%d] to be list type", i);
01197       return false;
01198     }
01199     if (coeff_row.size() != 6)
01200     {
01201       ROS_ERROR("Expected FT param calibration_coeff[%d] to have 6 elements", i);
01202       return false;
01203     }
01204     
01205     for (int j=0; j<6; ++j)
01206     {
01207       if (coeff_row[j].getType() != XmlRpc::XmlRpcValue::TypeDouble)
01208       {
01209         ROS_ERROR("Expected FT param calibration_coeff[%d,%d] to be floating point type", i,j);
01210         return false;
01211       } else {
01212         calibration_coeff(i,j) = static_cast<double>(coeff_row[j]);
01213       }
01214     }
01215   }
01216 
01217   return true;
01218 }
01219 
01220 
01221 const unsigned WG06::NUM_FT_CHANNELS;
01222 const unsigned WG06::MAX_FT_SAMPLES;


ethercat_hardware
Author(s): Rob Wheeler , Derek King
autogenerated on Thu Jun 6 2019 19:46:32