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


netft_utils
Author(s): Alex von Sternberg , Derek King, Andy Zelenak
autogenerated on Sat Jul 15 2017 02:45:58