netft.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_ethercat_hardware/netft.h"
00036 
00037 
00038 // Product code = NetFT
00039 // NetFT is not an EtherCAT device.  As such, it does not have EtherCAT productID 
00040 // will not be loaded on demand because EML finds device with matching product ID.
00041 // However, newer version of ethercat_hardware will use rosparam to load non-ethercat 
00042 // plugins.  Non-ethercat plugins can take commands and provide data to realtime system
00043 //PLUGINLIB_REGISTER_CLASS(NetFT, netft_ethercat_hardware::NetFT, EthercatDevice);
00044 PLUGINLIB_DECLARE_CLASS(netft_ethercat_hardware, NetFT, netft_ethercat_hardware::NetFT, EthercatDevice);
00045 
00046 
00047 namespace netft_ethercat_hardware
00048 {
00049 
00050 
00051 NetFT::NetFT() :
00052   hw_(NULL),
00053   netft_driver_(NULL),
00054   pub_(NULL),
00055   pub_old_(NULL),
00056   publish_period_(0.1)
00057 {
00058 }
00059 
00060 NetFT::~NetFT()
00061 {
00062   delete netft_driver_;
00063   delete pub_;
00064   delete pub_old_;
00065 }
00066 
00067 void NetFT::construct(ros::NodeHandle &nh)
00068 {
00069   nh_ = nh;
00070 }
00071 
00072 int NetFT::initialize(pr2_hardware_interface::HardwareInterface *hw, bool)
00073 {
00074   hw_ = hw;
00075 
00076   // Register analog inputs
00077 
00078   // Get device IP address from rosparam
00079   std::string address;
00080   if (!nh_.getParam("address", address))
00081   {
00082     ROS_ERROR("netft_ethercat_hardware : No param 'address' in namespace %s", nh_.getNamespace().c_str());
00083     return -1;
00084   }
00085  
00086   // Use rosparm when for select name of AnalogIn
00087   if (!nh_.getParam("analog_in_name", analog_in_.name_))
00088   {
00089     ROS_ERROR("netft_ethercat_hardware : No param 'analog_in_name' in namespace %s", nh_.getNamespace().c_str());
00090     return -1;
00091   }
00092 
00093   if (hw && !hw->addAnalogIn(&analog_in_))
00094   {
00095     ROS_FATAL("netft_ethercat_hardware : An analog input with the name '%s' already exists.", 
00096               analog_in_.name_.c_str());
00097     return -1;
00098   }
00099   analog_in_.state_.state_.resize(6);  // Reserve location for 6 analog values
00100 
00101   
00102   double publish_period;
00103   if (!nh_.getParam("ros_publish_period", publish_period))
00104   {
00105     ROS_ERROR("netft_ethercat_hardware : No param 'ros_publish_period' in namespace %s", nh_.getNamespace().c_str());
00106     return -1;
00107   }
00108   publish_period_ = ros::Duration(publish_period);
00109   last_publish_time_ = ros::Time::now();
00110 
00111   bool publish_wrench = false;
00112   if (!nh_.getParam("publish_wrench", publish_wrench))
00113   {
00114     publish_wrench = false;
00115   }
00116 
00117   if (publish_wrench)
00118   {
00119     ROS_WARN("Publishing NetFT data as geometry_msgs::Wrench is deprecated");
00120     pub_old_ = new realtime_tools::RealtimePublisher<geometry_msgs::Wrench>(nh_, "netft_data", 2);
00121   }
00122   else 
00123   {
00124     pub_ = new realtime_tools::RealtimePublisher<geometry_msgs::WrenchStamped>(nh_, "netft_data", 2);
00125   }
00126 
00127   try 
00128   {
00129     netft_driver_ = new netft_rdt_driver::NetFTRDTDriver(address);
00130   }
00131   catch (std::exception &e)
00132   {
00133     ROS_ERROR("netft_ethercat_hardware : Error constructing NetFT driver : %s", e.what());
00134     return -1;
00135   }
00136 
00137   return 0;
00138 }
00139 
00140 
00141 bool NetFT::unpackState(unsigned char *, unsigned char *)
00142 {
00143   // Take most recent UDP data and move to analog inputs
00144   geometry_msgs::WrenchStamped data;
00145   netft_driver_->getData(data);
00146 
00147   // Update analog inputs
00148   analog_in_.state_.state_.resize(6);
00149   analog_in_.state_.state_[0] = data.wrench.force.x;
00150   analog_in_.state_.state_[1] = data.wrench.force.y;
00151   analog_in_.state_.state_[2] = data.wrench.force.z;
00152   analog_in_.state_.state_[3] = data.wrench.torque.x;
00153   analog_in_.state_.state_[4] = data.wrench.torque.y;
00154   analog_in_.state_.state_[5] = data.wrench.torque.z;
00155 
00156   if ( (hw_->current_time_ - last_publish_time_) > publish_period_ )
00157   {
00158     last_publish_time_ += publish_period_;
00159     should_publish_ = true;
00160   }
00161   
00162   if (should_publish_)
00163   {
00164     if (tryPublish(data) || tryPublishOld(data))
00165     {
00166       should_publish_ = false;
00167     }
00168   }
00169 
00170   return true;
00171 }
00172 
00173 bool NetFT::tryPublish(const geometry_msgs::WrenchStamped &data)
00174 {
00175   if (pub_ == NULL)
00176   {
00177     return false;
00178   }
00179   if (!pub_->trylock())
00180   {
00181     return false;
00182   }
00183    
00184   pub_->msg_ = data;
00185   pub_->unlockAndPublish();
00186   return true;
00187 }
00188 
00189 
00190 bool NetFT::tryPublishOld(const geometry_msgs::WrenchStamped &data)
00191 {
00192   if (pub_old_ == NULL)
00193   {
00194     return false;
00195   }
00196   if (!pub_old_->trylock())
00197   {
00198     return false;
00199   }
00200 
00201   pub_old_->msg_ = data.wrench;
00202   pub_old_->unlockAndPublish();
00203   return true;
00204 }
00205 
00206 
00207 
00208 void NetFT::collectDiagnostics(EthercatCom *)
00209 {
00210   // Don't allow default collection of diagnostics
00211 }
00212 
00213 void NetFT::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *)
00214 {
00215   netft_driver_->diagnostics(d);
00216   d.add("AnalogIn name", analog_in_.name_);
00217 }
00218   
00219 };


netft_ethercat_hardware
Author(s): Derek King
autogenerated on Tue Apr 22 2014 19:17:57