$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_example_controllers/netft_example_controller.h" 00036 #include "pluginlib/class_list_macros.h" 00037 #include <math.h> 00038 #include <boost/foreach.hpp> 00039 #include <string> 00040 #include <algorithm> 00041 00042 PLUGINLIB_DECLARE_CLASS(netft_example_controllers, NetFTExampleController, netft_example_controllers::NetFTExampleController, pr2_controller_interface::Controller) 00043 00044 00045 namespace netft_example_controllers 00046 { 00047 00048 NetFTExampleController::NetFTExampleController() : 00049 max_force_(0.0), 00050 max_torque_(0.0), 00051 analog_in_(NULL), 00052 pub_cycle_count_(0), 00053 should_publish_(false) 00054 { 00055 // Nothing 00056 } 00057 00058 00059 NetFTExampleController::~NetFTExampleController() 00060 { 00061 // Nothing 00062 } 00063 00064 00065 bool NetFTExampleController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &node) 00066 { 00067 if (!robot) 00068 return false; 00069 00070 std::string analog_in_name; 00071 if (!node.getParam("analog_in_name", analog_in_name)) 00072 { 00073 ROS_ERROR("NetFTExampleController: No \"analog_in_name\" found on parameter namespace: %s", 00074 node.getNamespace().c_str()); 00075 return false; 00076 } 00077 00078 pr2_hardware_interface::HardwareInterface* hw = robot->model_->hw_; 00079 analog_in_ = hw->getAnalogIn(analog_in_name); 00080 if (analog_in_ == NULL) 00081 { 00082 ROS_ERROR("NetFTExampleController: Cannot find AnalogIn named \"%s\"", 00083 analog_in_name.c_str()); 00084 BOOST_FOREACH(const pr2_hardware_interface::AnalogInMap::value_type &v, hw->analog_ins_) 00085 { 00086 ROS_INFO("AnalogIn : %s", v.first.c_str()); 00087 } 00088 return false; 00089 } 00090 ROS_INFO("NetFTExampleController: Using AnalogIn named \"%s\"", analog_in_name.c_str()); 00091 00092 // Initialize realtime publisher to publish to ROS topic 00093 pub_.init(node, "force_torque_stats", 2); 00094 00095 return true; 00096 } 00097 00098 00099 void NetFTExampleController::starting() 00100 { 00101 // Nothing to do here 00102 } 00103 00104 00105 void NetFTExampleController::update() 00106 { 00107 if (analog_in_->state_.state_.size() != 6) 00108 { 00109 ROS_ERROR_THROTTLE(5.0, "NetFTExampleController: AnalogInput is has unexpected size %d", 00110 int(analog_in_->state_.state_.size())); 00111 return; 00112 } 00113 00114 double fx = analog_in_->state_.state_[0]; 00115 double fy = analog_in_->state_.state_[1]; 00116 double fz = analog_in_->state_.state_[2]; 00117 double tx = analog_in_->state_.state_[3]; 00118 double ty = analog_in_->state_.state_[4]; 00119 double tz = analog_in_->state_.state_[5]; 00120 00121 double abs_force = sqrt( fx*fx + fy*fy + fz*fz ); 00122 double abs_torque = sqrt( tx*tx + ty*ty + tz*tz ); 00123 max_force_ = std::max(max_force_, abs_force); 00124 max_torque_ = std::max(max_torque_, abs_torque); 00125 00126 // Publish data in ROS message every 10 cycles (about 100Hz) 00127 if (++pub_cycle_count_ > 10) 00128 { 00129 should_publish_ = true; 00130 pub_cycle_count_ = 0; 00131 } 00132 00133 if (should_publish_ && pub_.trylock()) 00134 { 00135 should_publish_ = false; 00136 pub_.msg_.abs_force = abs_force; 00137 pub_.msg_.max_force = max_force_; 00138 pub_.msg_.abs_torque = abs_torque; 00139 pub_.msg_.max_torque = max_torque_; 00140 pub_.unlockAndPublish(); 00141 } 00142 } 00143 00144 }//namespace netft_example_controllers