network_detector.cpp
Go to the documentation of this file.
1 #include <netinet/ip.h>
2 #include <sys/ioctl.h>
3 #include <net/if.h>
4 #include <errno.h>
5 
6 #include <string>
7 
8 #include "ros/ros.h"
9 #include "std_msgs/Bool.h"
10 
11 static int socket_fd = -1;
12 
13 bool initSocket()
14 {
15  socket_fd = socket( AF_INET, SOCK_DGRAM, 0 );
16  if( socket_fd != -1 )
17  return true;
18  else
19  return false;
20 }
21 
22 bool interfaceIsRunning( std::string interface_name )
23 {
24  struct ifreq ifr;
25 
26  strcpy( ifr.ifr_name, interface_name.c_str() );
27  if( ioctl( socket_fd, SIOCGIFFLAGS, &ifr ) < 0 )
28  {
29  static std::string last_warning;
30  std::string warning = "Query of interface '" + interface_name + "' failed: '" + strerror( errno ) + "' Presuming down.";
31  if( warning != last_warning )
32  {
33  ROS_WARN("%s", warning.c_str() );
34  }
35  last_warning = warning;
36  return false;
37  }
38  bool running = (ifr.ifr_flags & IFF_RUNNING);
39  bool up = (ifr.ifr_flags & IFF_UP);
40 
41  return up && running;
42 }
43 
44 int main( int argc, char **argv )
45 {
46  ros::init( argc, argv, "network_detector" );
47  ros::NodeHandle node;
48  std::string interface_name;
49  if( !ros::param::get( "~interface_name", interface_name ))
50  {
51  ROS_FATAL( "No parameter 'interface_name' specified. Don't know which interface to watch. Exiting." );
52  exit(1);
53  }
54  ros::Publisher running_pub = node.advertise<std_msgs::Bool>( "network/connected", 0, true );
55  int loop_count;
56  bool first_time = true;
57  bool was_running = false;
58  float inner_loop_hertz = 4;
59  ros::Rate loop_rate( inner_loop_hertz );
60  if( !initSocket() )
61  {
62  ROS_FATAL( "Failed to open socket for network ioctl: '%s'. Exiting.",
63  strerror( errno ));
64  exit(1);
65  }
66  while( ros::ok() )
67  {
68  bool is_running = interfaceIsRunning( interface_name );
69  if( is_running != was_running || first_time || loop_count > inner_loop_hertz * 5 )
70  {
71  if( is_running != was_running )
72  {
73  ROS_INFO( "Interface '%s' %s.", interface_name.c_str(), (is_running ? "connected" : "disconnected") );
74  }
75 
76  std_msgs::Bool msg;
77  msg.data = is_running;
78  running_pub.publish( msg );
79 
80  loop_count = 0;
81  first_time = false;
82  }
83  ros::spinOnce();
84  loop_rate.sleep();
85  loop_count++;
86  was_running = is_running;
87  }
88 }
#define ROS_FATAL(...)
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_WARN(...)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
static int socket_fd
#define ROS_INFO(...)
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
bool interfaceIsRunning(std::string interface_name)
bool initSocket()
ROSCPP_DECL void spinOnce()
int main(int argc, char **argv)


pr2_computer_monitor
Author(s): Kevin Watts (watts@willowgarage.com)
autogenerated on Tue Jun 1 2021 02:50:51