netft_node.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 
00040 #include "ros/ros.h"
00041 #include "netft_rdt_driver/netft_rdt_driver.h"
00042 #include "geometry_msgs/WrenchStamped.h"
00043 #include "diagnostic_msgs/DiagnosticArray.h"
00044 #include "diagnostic_updater/DiagnosticStatusWrapper.h"
00045 #include <unistd.h>
00046 #include <iostream>
00047 #include <memory>
00048 #include <boost/program_options.hpp>
00049 
00050 namespace po = boost::program_options;
00051 using namespace std;
00052 
00053 
00054 int main(int argc, char **argv)
00055 { 
00056   ros::init(argc, argv, "netft_node");
00057   ros::NodeHandle nh;
00058 
00059   float pub_rate_hz;
00060   string address;
00061 
00062   po::options_description desc("Options");
00063   desc.add_options()
00064     ("help", "display help")
00065     ("rate", po::value<float>(&pub_rate_hz)->default_value(100.0), "set publish rate (in hertz)")
00066     ("wrench", "publish older Wrench message type instead of WrenchStamped")
00067     ("address", po::value<string>(&address), "IP address of NetFT box")
00068     ;
00069      
00070   po::positional_options_description p;
00071   p.add("address",  1);
00072 
00073   po::variables_map vm;
00074   po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
00075   po::notify(vm);
00076 
00077   if (vm.count("help"))
00078   {
00079     cout << desc << endl;
00080     //usage(progname);
00081     exit(EXIT_SUCCESS);
00082   }      
00083 
00084   if (!vm.count("address"))
00085   {
00086     cout << desc << endl;
00087     cerr << "Please specify address of NetFT" << endl;
00088     exit(EXIT_FAILURE);
00089   }
00090 
00091   bool publish_wrench = false;
00092   if (vm.count("wrench"))
00093   {
00094     publish_wrench = true;
00095     ROS_WARN("Publishing NetFT data as geometry_msgs::Wrench is deprecated");
00096   }
00097 
00098   std::auto_ptr<netft_rdt_driver::NetFTRDTDriver> netft(new netft_rdt_driver::NetFTRDTDriver(address));
00099   ros::Publisher pub;
00100   if (publish_wrench)
00101   {
00102     pub = nh.advertise<geometry_msgs::Wrench>("netft_data", 100);
00103   }
00104   else 
00105   {
00106     pub = nh.advertise<geometry_msgs::WrenchStamped>("netft_data", 100);
00107   }
00108   ros::Rate pub_rate(pub_rate_hz);
00109   geometry_msgs::WrenchStamped data;
00110 
00111   ros::Duration diag_pub_duration(1.0);
00112   ros::Publisher diag_pub = nh.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 2);
00113   diagnostic_msgs::DiagnosticArray diag_array;
00114   diag_array.status.reserve(1);
00115   diagnostic_updater::DiagnosticStatusWrapper diag_status;
00116   ros::Time last_diag_pub_time(ros::Time::now());
00117 
00118   while (ros::ok())
00119   {
00120     if (netft->waitForNewData())
00121     {
00122       netft->getData(data);
00123       if (publish_wrench) 
00124       {
00125         //geometry_msgs::Wrench(data.wrench);
00126         pub.publish(data.wrench);
00127       }
00128       else 
00129       {
00130         pub.publish(data);
00131       }
00132     }
00133     
00134     ros::Time current_time(ros::Time::now());
00135     if ( (current_time - last_diag_pub_time) > diag_pub_duration )
00136     {
00137       diag_array.status.clear();
00138       netft->diagnostics(diag_status);
00139       diag_array.status.push_back(diag_status);
00140       diag_array.header.stamp = ros::Time::now();
00141       diag_pub.publish(diag_array);
00142       last_diag_pub_time = current_time;
00143     }
00144     
00145     ros::spinOnce();
00146     pub_rate.sleep();
00147   }
00148   
00149   return 0;
00150 }


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