find_camera.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 
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   // Address to receive at.
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   // Address to receive at.
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   // Send a broadcast.
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 }


prosilica_camera
Author(s): Maintained by William Woodall - wwoodall@willowgarage.com
autogenerated on Mon Oct 6 2014 03:56:56