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 }
