netft_rdt_driver.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 "netft_rdt_driver/netft_rdt_driver.h"
00036 #include <stdint.h>
00037 #include <exception>
00038 
00039 using boost::asio::ip::udp;
00040 
00041 namespace netft_rdt_driver
00042 {
00043 
00044 struct RDTRecord
00045 {
00046   uint32_t rdt_sequence_;
00047   uint32_t ft_sequence_;
00048   uint32_t status_;
00049   int32_t fx_;
00050   int32_t fy_;
00051   int32_t fz_;
00052   int32_t tx_;
00053   int32_t ty_;
00054   int32_t tz_;
00055 
00056   enum {RDT_RECORD_SIZE = 36};
00057   void unpack(const uint8_t *buffer);
00058   static uint32_t unpack32(const uint8_t *buffer);
00059 };
00060 
00061 uint32_t RDTRecord::unpack32(const uint8_t *buffer)
00062 {
00063   return
00064     ( uint32_t(buffer[0]) << 24) |     
00065     ( uint32_t(buffer[1]) << 16) |     
00066     ( uint32_t(buffer[2]) << 8 ) |     
00067     ( uint32_t(buffer[3]) << 0 ) ;
00068 }
00069 
00070 void RDTRecord::unpack(const uint8_t *buffer)
00071 {
00072   rdt_sequence_ = unpack32(buffer + 0);
00073   ft_sequence_  = unpack32(buffer + 4);
00074   status_       = unpack32(buffer + 8);
00075   fx_ = unpack32(buffer + 12);
00076   fy_ = unpack32(buffer + 16);
00077   fz_ = unpack32(buffer + 20);
00078   tx_ = unpack32(buffer + 24);
00079   ty_ = unpack32(buffer + 28);
00080   tz_ = unpack32(buffer + 32);
00081 }
00082 
00083 
00084 struct RDTCommand
00085 {
00086   uint16_t command_header_;
00087   uint16_t command_;
00088   uint32_t sample_count_;
00089 
00090   RDTCommand() : command_header_(HEADER)
00091   {
00092     // empty
00093   }
00094 
00095   enum {HEADER=0x1234};
00096 
00097   // Possible values for command_
00098   enum {
00099     CMD_STOP_STREAMING=0, 
00100     CMD_START_HIGH_SPEED_STREAMING=2,
00101     // More command values are available but are not used by this driver
00102   };
00103 
00104   // Special values for sample count
00105   enum { INFINITE_SAMPLES=0 };
00106 
00107   enum {RDT_COMMAND_SIZE = 8};
00108 
00110   //  Buffer should be RDT_COMMAND_SIZE
00111   void pack(uint8_t *buffer) const;
00112 };
00113 
00114 void RDTCommand::pack(uint8_t *buffer) const
00115 {
00116   // Data is big-endian
00117   buffer[0] = (command_header_ >> 8) & 0xFF;
00118   buffer[1] = (command_header_ >> 0) & 0xFF;
00119   buffer[2] = (command_ >> 8) & 0xFF;
00120   buffer[3] = (command_ >> 0) & 0xFF;
00121   buffer[4] = (sample_count_ >> 8) & 0xFF;
00122   buffer[5] = (sample_count_ >> 0) & 0xFF;
00123   buffer[6] = (sample_count_ >> 8) & 0xFF;
00124   buffer[7] = (sample_count_ >> 0) & 0xFF;
00125 }
00126 
00127 
00128 NetFTRDTDriver::NetFTRDTDriver(const std::string &address) :
00129   address_(address),
00130   socket_(io_service_),
00131   stop_recv_thread_(false),
00132   recv_thread_running_(false),
00133   packet_count_(0),
00134   lost_packets_(0),
00135   out_of_order_count_(0),
00136   seq_counter_(0),
00137   diag_packet_count_(0),
00138   last_diag_pub_time_(ros::Time::now()),
00139   last_rdt_sequence_(0),
00140   system_status_(0)
00141 {
00142   // Construct UDP socket
00143   udp::endpoint netft_endpoint( boost::asio::ip::address_v4::from_string(address), RDT_PORT);
00144   socket_.open(udp::v4());
00145   socket_.connect(netft_endpoint);
00146   
00147   // TODO : Get/Set Force/Torque scale for device
00148   // Force/Sclae is based on counts per force/torque value from device
00149   // these value are manually read from device webserver, but in future they 
00150   // may be collected using http get requests
00151   static const double counts_per_force = 1000000;  
00152   static const double counts_per_torque = 1000000;
00153   force_scale_ = 1.0 / counts_per_force;
00154   torque_scale_ = 1.0 / counts_per_torque;
00155 
00156   // Start receive thread  
00157   recv_thread_ = boost::thread(&NetFTRDTDriver::recvThreadFunc, this);
00158 
00159   // Since start steaming command is sent with UDP packet,
00160   // the packet could be lost, retry startup 10 times before giving up
00161   for (int i=0; i<10; ++i)
00162   {
00163     startStreaming();
00164     if (waitForNewData())
00165       break;
00166   }
00167   { boost::unique_lock<boost::mutex> lock(mutex_);
00168     if (packet_count_ == 0)
00169     {
00170       throw std::runtime_error("No data received from NetFT device");
00171     }
00172   }
00173 
00174 }
00175 
00176 
00177 NetFTRDTDriver::~NetFTRDTDriver()
00178 {
00179   // TODO stop transmission, 
00180   // stop thread
00181   stop_recv_thread_ = true;
00182   if (!recv_thread_.timed_join(boost::posix_time::time_duration(0,0,1,0)))
00183   {
00184     ROS_WARN("Interrupting recv thread");
00185     recv_thread_.interrupt();
00186     if (!recv_thread_.timed_join(boost::posix_time::time_duration(0,0,1,0)))
00187     {
00188       ROS_WARN("Failed second join to recv thread");
00189     }
00190   }
00191   socket_.close();
00192 }
00193 
00194 
00195 bool NetFTRDTDriver::waitForNewData()
00196 {
00197   // Wait upto 100ms for new data
00198   bool got_new_data = false;
00199   {
00200     boost::mutex::scoped_lock lock(mutex_);
00201     unsigned current_packet_count = packet_count_;
00202     condition_.timed_wait(lock, boost::posix_time::milliseconds(100));    
00203     got_new_data = packet_count_ != current_packet_count;
00204   }
00205 
00206   return got_new_data;
00207 }
00208 
00209 
00210 void NetFTRDTDriver::startStreaming(void)
00211 {
00212   // Command NetFT to start data transmission
00213   RDTCommand start_transmission; 
00214   start_transmission.command_ = RDTCommand::CMD_START_HIGH_SPEED_STREAMING;
00215   start_transmission.sample_count_ = RDTCommand::INFINITE_SAMPLES;
00216   // TODO change buffer into boost::array
00217   uint8_t buffer[RDTCommand::RDT_COMMAND_SIZE];
00218   start_transmission.pack(buffer);
00219   socket_.send(boost::asio::buffer(buffer, RDTCommand::RDT_COMMAND_SIZE)); 
00220 }
00221 
00222 
00223 
00224 void NetFTRDTDriver::recvThreadFunc()
00225 {
00226   try {
00227     recv_thread_running_ = true;
00228     RDTRecord rdt_record;
00229     geometry_msgs::WrenchStamped tmp_data;
00230     uint8_t buffer[RDTRecord::RDT_RECORD_SIZE+1];
00231     while (!stop_recv_thread_)
00232     {
00233       size_t len = socket_.receive(boost::asio::buffer(buffer, RDTRecord::RDT_RECORD_SIZE+1));
00234       if (len != RDTRecord::RDT_RECORD_SIZE)
00235       {
00236         ROS_WARN("Receive size of %d bytes does not match expected size of %d", 
00237                  int(len), int(RDTRecord::RDT_RECORD_SIZE));
00238       }
00239       else
00240       {
00241         rdt_record.unpack(buffer);
00242         if (rdt_record.status_ != 0)
00243         {
00244           // Latch any system status error code
00245           boost::unique_lock<boost::mutex> lock(mutex_);
00246           system_status_ = rdt_record.status_;
00247         }
00248         int32_t seqdiff = int32_t(rdt_record.rdt_sequence_ - last_rdt_sequence_);
00249         last_rdt_sequence_ = rdt_record.rdt_sequence_;
00250         if (seqdiff < 1)
00251         { boost::unique_lock<boost::mutex> lock(mutex_);
00252           // Don't use data that is old
00253           ++out_of_order_count_;
00254         }
00255         else 
00256         {
00257           tmp_data.header.seq = seq_counter_++;
00258           tmp_data.header.stamp = ros::Time::now();
00259           tmp_data.header.frame_id = "base_link";
00260           tmp_data.wrench.force.x = double(rdt_record.fx_) * force_scale_;
00261           tmp_data.wrench.force.y = double(rdt_record.fy_) * force_scale_;
00262           tmp_data.wrench.force.z = double(rdt_record.fz_) * force_scale_;
00263           tmp_data.wrench.torque.x = double(rdt_record.tx_) * torque_scale_;
00264           tmp_data.wrench.torque.y = double(rdt_record.ty_) * torque_scale_;
00265           tmp_data.wrench.torque.z = double(rdt_record.tz_) * torque_scale_;
00266           { boost::unique_lock<boost::mutex> lock(mutex_);
00267             new_data_ = tmp_data;
00268             lost_packets_ += (seqdiff - 1);
00269             ++packet_count_;
00270             condition_.notify_all();
00271           }
00272         }
00273       }
00274     } // end while
00275   }
00276   catch (std::exception &e)
00277   {    
00278     recv_thread_running_ = false;
00279     { boost::unique_lock<boost::mutex> lock(mutex_);
00280       recv_thread_error_msg_ = e.what();
00281     }
00282   }
00283 }
00284 
00285 
00286 void NetFTRDTDriver::getData(geometry_msgs::WrenchStamped &data)
00287 {
00288   { boost::unique_lock<boost::mutex> lock(mutex_);
00289     data = new_data_;
00290   }  
00291 }
00292 
00293 
00294 void NetFTRDTDriver::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
00295 {
00296   // Publish diagnostics
00297   d.name = "NetFT RDT Driver : " + address_;
00298   
00299   d.summary(d.OK, "OK");
00300   d.hardware_id = "0";
00301 
00302   if (diag_packet_count_ == packet_count_)
00303   {
00304     d.mergeSummary(d.ERROR, "No new data in last second");
00305   }
00306 
00307   if (!recv_thread_running_)
00308   {
00309     d.mergeSummaryf(d.ERROR, "Receive thread has stopped : %s", recv_thread_error_msg_.c_str());
00310   }
00311 
00312   if (system_status_ != 0)
00313   {
00314     d.mergeSummaryf(d.ERROR, "NetFT reports error 0x%08x", system_status_);
00315   }
00316 
00317   ros::Time current_time(ros::Time::now());
00318   double recv_rate = double(int32_t(packet_count_ - diag_packet_count_)) / (current_time - last_diag_pub_time_).toSec();
00319     
00320   d.clear();
00321   d.addf("IP Address", "%s", address_.c_str());
00322   d.addf("System status", "0x%08x", system_status_);
00323   d.addf("Good packets", "%u", packet_count_);
00324   d.addf("Lost packets", "%u", lost_packets_);
00325   d.addf("Out-of-order packets", "%u", out_of_order_count_);
00326   d.addf("Recv rate (pkt/sec)", "%.2f", recv_rate);
00327   d.addf("Force scale (N/bit)", "%f", force_scale_);
00328   d.addf("Torque scale (Nm/bit)", "%f", torque_scale_);
00329 
00330   geometry_msgs::WrenchStamped data;
00331   getData(data);
00332   d.addf("Force X (N)",   "%f", data.wrench.force.x);
00333   d.addf("Force Y (N)",   "%f", data.wrench.force.y);
00334   d.addf("Force Z (N)",   "%f", data.wrench.force.z);
00335   d.addf("Torque X (Nm)", "%f", data.wrench.torque.x);
00336   d.addf("Torque Y (Nm)", "%f", data.wrench.torque.y);
00337   d.addf("Torque Z (Nm)", "%f", data.wrench.torque.z);
00338 
00339   last_diag_pub_time_ = current_time;
00340   diag_packet_count_ = packet_count_;
00341 }
00342 
00343 
00344 } // end namespace netft_rdt_driver
00345 


netft_rdt_driver
Author(s): Derek King
autogenerated on Tue Apr 22 2014 19:17:54