$search
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 };