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_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   // 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 selectively enabled / disabled    
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; //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   // FT provides 6 values : 3 Forces + 3 Torques
00352   ft_raw_analog_in_.state_.state_.resize(6); 
00353   // FT usually provides 3-4 new samples per cycle
00354   force_torque_.state_.samples_.reserve(4);
00355   force_torque_.state_.good_ = true;
00356 
00357   // For now publish RAW F/T values for engineering purposes.  In future this publisher may be disabled by default.
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   // Allocate space for raw f/t data values
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       // If we have ft_params, publish F/T values.  
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   // TODO: do not use direct access to device.  Not safe while realtime loop is running
00402   // ALSO, this leaks memory
00403   EthercatDirectCom *com = new EthercatDirectCom(EtherCAT_DataLinkLayer::instance());
00404 
00405   // Add soft-processors to list
00406   soft_processor_.add(&mailbox_, actuator_.name_, "pressure", 0xA000, 0x249);
00407   soft_processor_.add(&mailbox_, actuator_.name_, "accel", 0xB000, 0x24A);
00408 
00409   // Start services
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) :  // Has FT sensor and accelerometer
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     //goto end;  // all other tasks should not be effected by bad pressure sensor data
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     // If pressure sensor is not enabled don't attempt to do anything with pressure data
00516     return true;
00517   }
00518 
00519   if (!verifyChecksum(pressure_buf, pressure_size_))
00520   {
00521     ++pressure_checksum_error_count_;
00522     if (false /* debugging */)
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   // Only most recent 4 samples of accelerometer data is available in status data
00581   // 4 samples will be enough with realtime loop running at 1kHz and accelerometer running at 3kHz
00582   // If count is greater than 4, then some data has been "missed".
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   // Apply gains / offset to F/T raw analog inputs
00638   // Also, make sure values are within bounds.  
00639   // Out-of-bound values indicate a broken sensor or an overload.
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   // Vhalf ADC measurement should be amost half the ADC reference voltage
00652   // For a 16bit ADC the Vhalf value should be about (1<<16)/2
00653   // If Vhalf value is not close to this, this could mean 1 of 2 things:
00654   //   1. WG035 electronics are damaged some-how 
00655   //   2. WG035 is not present or disconnected gripper MCB 
00656   if ( abs( int(sample.vhalf_) - FT_VHALF_IDEAL) > FT_VHALF_RANGE )
00657   {
00658     if ((sample.vhalf_ == 0x0000) || (sample.vhalf_ == 0xFFFF))
00659     {
00660       // When WG035 MCB is not present the DATA line floats low or high and all 
00661       // reads through SPI interface return 0.
00662       ft_disconnected_ = true;
00663     }
00664     else 
00665     {
00666       ft_vhalf_error_ = true;
00667     }
00668   }
00669 
00670   // Apply coeffiecient matrix multiplication to raw inputs to get force/torque values
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   // Fill in raw analog output with most recent data sample, (might become deprecated?)
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   // Also, if number of new_samples is ever 0, then there is also an error
00720   if (usable_samples == 0)
00721   {
00722     ft_sampling_rate_error_ = true;
00723   }
00724 
00725   // Make room in data structure for more f/t samples
00726   ft_state.samples_.resize(usable_samples);
00727 
00728   // If any f/t channel is overload or the sampling rate is bad, there is an error.
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     // samples are stored in status data, so that newest sample is at index 0.
00737     // this is the reverse of the order data is stored in hardware_interface::ForceTorque buffer.
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   // Put newest sample into analog vector for controllers (deprecated)
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   // Put all new samples in buffer and publish it
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       // put data into message so oldest data is first element
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   // Put newest sample in realtime publisher
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   // If this returns false, it will cause motors to halt.
00788   // Return "good_" state of sensor, unless halt_on_error is false. 
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     // perform f/t sample
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   // Board revB=1 and revA=0 does not have accelerometer
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   // nothing else to do here 
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) :  // Has FT sensor and accelerometer
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     // look at pressure sensor value to detect any damaged cables, or possibly missing sensor
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     // if no pressure sensor regions are return acceptable data, then its possible that the
00928     // pressure sensor is not connected at all.  This is just a warning, because on many robots
00929     // the pressure sensor may not be installed
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       // At least one pressure sensor seems to be present ...       
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     { // put right and left finger data in dianostics
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   //d.addf("F/T sample count", "%llu", ft_sample_count_);
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]);  //use newest data sample
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   // Initial offset = 0.0
01065   // Gains = 1.0 
01066   // Calibration coeff = identity matrix
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;


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Jan 2 2014 11:39:31