00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <ros/console.h>
00036 #include <sys/types.h>
00037 #include <sys/socket.h>
00038 #include <ifaddrs.h>
00039 #include <netinet/ip.h>
00040 #include <netinet/in.h>
00041 #include <arpa/inet.h>
00042 #include <errno.h>
00043 #include <string.h>
00044
00045 int receive_socket()
00046 {
00047 int insock;
00048 struct sockaddr_in recvaddr;
00049
00050 if ((insock = socket(PF_INET,SOCK_DGRAM,0)) == -1)
00051 {
00052 ROS_ERROR("Error opening input socket.");
00053 return -1;
00054 }
00055
00056
00057 memset(&recvaddr, 0, sizeof(recvaddr));
00058 recvaddr.sin_family = AF_INET;
00059 recvaddr.sin_addr.s_addr = INADDR_BROADCAST;
00060 recvaddr.sin_port = htons(1234);
00061
00062 if (bind(insock, (struct sockaddr*) &recvaddr, sizeof recvaddr) == -1)
00063 {
00064 ROS_ERROR("Can't bind to socket to receive.");
00065 close(insock);
00066 return -1;
00067 }
00068
00069 return insock;
00070 }
00071
00072 int send_query(struct ifaddrs *iface, int srcport)
00073 {
00074 int outsock;
00075 int port = 3956;
00076 int broadcast=1;
00077 struct sockaddr_in sendaddr;
00078 struct sockaddr_in recvaddr;
00079
00080 if ((outsock = socket(PF_INET,SOCK_DGRAM,0)) == -1)
00081 {
00082 ROS_ERROR("Error opening output socket.");
00083 return -1;
00084 }
00085
00086
00087 memset(&sendaddr, 0, sizeof(sendaddr));
00088 sendaddr.sin_family = AF_INET;
00089 sendaddr.sin_addr.s_addr = ((sockaddr_in *) iface->ifa_addr)->sin_addr.s_addr;
00090 sendaddr.sin_port = htons(srcport);
00091
00092 if (bind(outsock, (struct sockaddr*) &sendaddr, sizeof sendaddr) == -1)
00093 {
00094 ROS_ERROR("Can't bind to socket on interface %s.", iface->ifa_name);
00095 close(outsock);
00096 return -1;
00097 }
00098
00099
00100 if ((setsockopt(outsock,SOL_SOCKET,SO_BROADCAST, &broadcast,sizeof broadcast)) == -1)
00101 {
00102 ROS_ERROR("Can't set broadcast flag on interface %s.", iface->ifa_name);
00103 close(outsock);
00104 return -1;
00105 }
00106
00107 memset(&recvaddr, 0, sizeof(recvaddr));
00108 recvaddr.sin_family = AF_INET;
00109 recvaddr.sin_port = htons(port);
00110 recvaddr.sin_addr.s_addr = INADDR_BROADCAST;
00111
00112 char outpkt[] = { 0x42, 0x11, 0x00, 0x02, 0x00, 0x00, 0x06, 0x26 };
00113
00114 if (sendto(outsock, outpkt, sizeof(outpkt) , 0, (struct sockaddr *)&recvaddr, sizeof(recvaddr)) != sizeof(outpkt))
00115 {
00116 ROS_ERROR("Failed sending packet on interface %s.", iface->ifa_name);
00117 close(outsock);
00118 return -1;
00119 }
00120
00121 close(outsock);
00122 return 0;
00123 }
00124
00125 void get_response(int fd)
00126 {
00127 ROS_INFO("Got a response on socket %i.", fd);
00128 }
00129
00130 int collect_replies(fd_set *master_set, int maxfd)
00131 {
00132 struct timeval tv;
00133 int retval;
00134 fd_set tmpset;
00135
00136 tv.tv_sec = 1;
00137 tv.tv_usec = 0;
00138
00139 while (1)
00140 {
00141 memcpy(&tmpset, master_set, sizeof(tmpset));
00142 retval = select(maxfd + 1, &tmpset, NULL, NULL, &tv);
00143
00144 if (retval < 0 && errno != EINTR)
00145 {
00146 ROS_ERROR("Select exited with an error code.");
00147 return -1;
00148 }
00149
00150 if (!retval)
00151 return 0;
00152
00153 ROS_INFO("Got packet %i.", retval);
00154 for (int i = 0; i <= maxfd && retval > 0; i++)
00155 if (FD_ISSET(i, &tmpset))
00156 {
00157 get_response(i);
00158 retval--;
00159 }
00160 }
00161 }
00162
00163 int get_local_port(int fd)
00164 {
00165 struct sockaddr localPort;
00166 socklen_t localPortLen;
00167
00168 if (getsockname(fd, &localPort, &localPortLen) == -1)
00169 {
00170 ROS_ERROR("Unable to get local port for socket.");
00171 return -1;
00172 }
00173
00174 return ntohs(((struct sockaddr_in *)&localPort)->sin_port);
00175 }
00176
00177 int main()
00178 {
00179 struct ifaddrs *ifaces = NULL;
00180 fd_set fdset;
00181 int maxfd = 0;
00182
00183 if (getifaddrs(&ifaces))
00184 {
00185 ROS_ERROR("Couldn't get interface list.");
00186 return 1;
00187 }
00188
00189 FD_ZERO(&fdset);
00190 maxfd = receive_socket();
00191 if (maxfd < 0)
00192 return 1;
00193 FD_SET(maxfd, &fdset);
00194
00195 int port = get_local_port(maxfd);
00196
00197 for (struct ifaddrs *curif = ifaces; curif; curif = curif->ifa_next)
00198 {
00199 if (curif->ifa_addr && curif->ifa_addr->sa_family == AF_INET)
00200 send_query(curif, port);
00201 }
00202
00203 freeifaddrs(ifaces);
00204
00205 return collect_replies(&fdset, maxfd) != 0;
00206 }