netft_example_controllers: netft_example_controllers: netft_example

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
netft_example_controller.cpp
