network_detector.cpp
Go to the documentation of this file.
00001 #include <netinet/ip.h>
00002 #include <sys/ioctl.h>
00003 #include <net/if.h>
00004 #include <errno.h>
00005 
00006 #include <string>
00007 
00008 #include "ros/ros.h"
00009 #include "std_msgs/Bool.h"
00010 
00011 static int socket_fd = -1;
00012 
00013 bool initSocket()
00014 {
00015   socket_fd = socket( AF_INET, SOCK_DGRAM, 0 );
00016   if( socket_fd != -1 )
00017     return true;
00018   else
00019     return false;
00020 }
00021 
00022 bool interfaceIsRunning( std::string interface_name )
00023 {
00024   struct ifreq ifr;
00025   
00026   strcpy( ifr.ifr_name, interface_name.c_str() );
00027   if( ioctl( socket_fd, SIOCGIFFLAGS, &ifr ) < 0 )
00028   {
00029     static std::string last_warning;
00030     std::string warning = "Query of interface '" + interface_name + "' failed: '" + strerror( errno ) + "'  Presuming down.";
00031     if( warning != last_warning )
00032     {
00033       ROS_WARN( warning.c_str() );
00034     }
00035     last_warning = warning;
00036     return false;
00037   }
00038   bool running = (ifr.ifr_flags & IFF_RUNNING);
00039   bool up = (ifr.ifr_flags & IFF_UP);
00040 
00041   return up && running;
00042 }
00043 
00044 int main( int argc, char **argv )
00045 {
00046   ros::init( argc, argv, "network_detector" );
00047   ros::NodeHandle node;
00048   std::string interface_name;
00049   if( !ros::param::get( "~interface_name", interface_name ))
00050   {
00051     ROS_FATAL( "No parameter 'interface_name' specified.  Don't know which interface to watch.  Exiting." );
00052     exit(1);
00053   }
00054   ros::Publisher running_pub = node.advertise<std_msgs::Bool>( "network/connected", 0, true );
00055   int loop_count;
00056   bool first_time = true;
00057   bool was_running = false;
00058   float inner_loop_hertz = 4;
00059   ros::Rate loop_rate( inner_loop_hertz );
00060   if( !initSocket() )
00061   {
00062     ROS_FATAL( "Failed to open socket for network ioctl: '%s'.  Exiting.",
00063                strerror( errno ));
00064     exit(1);
00065   }
00066   while( ros::ok() )
00067   {
00068     bool is_running = interfaceIsRunning( interface_name );
00069     if( is_running != was_running || first_time || loop_count > inner_loop_hertz * 5 )
00070     {
00071       if( is_running != was_running )
00072       {
00073         ROS_INFO( "Interface '%s' %s.", interface_name.c_str(), (is_running ? "connected" : "disconnected") );
00074       }
00075 
00076       std_msgs::Bool msg;
00077       msg.data = is_running;
00078       running_pub.publish( msg );
00079 
00080       loop_count = 0;
00081       first_time = false;
00082     }
00083     ros::spinOnce();
00084     loop_rate.sleep();
00085     loop_count++;
00086     was_running = is_running;
00087   }
00088 }


network_detector
Author(s): Dave Hershberger
autogenerated on Thu Apr 24 2014 15:35:25