network.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009, Willow Garage, Inc.
00003  *
00004  * Redistribution and use in source and binary forms, with or without
00005  * modification, are permitted provided that the following conditions are met:
00006  *   * Redistributions of source code must retain the above copyright notice,
00007  *     this list of conditions and the following disclaimer.
00008  *   * Redistributions in binary form must reproduce the above copyright
00009  *     notice, this list of conditions and the following disclaimer in the
00010  *     documentation and/or other materials provided with the distribution.
00011  *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
00012  *     contributors may be used to endorse or promote products derived from
00013  *     this software without specific prior written permission.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  */
00027 
00028 #include "config.h"
00029 #include "ros/network.h"
00030 #include "ros/file_log.h"
00031 #include "ros/exceptions.h"
00032 #include "ros/io.h"     // cross-platform headers needed
00033 #include <ros/console.h>
00034 #include <ros/assert.h>
00035 #ifdef HAVE_IFADDRS_H
00036   #include <ifaddrs.h>
00037 #endif
00038 
00039 #include <boost/lexical_cast.hpp>
00040 
00041 namespace ros
00042 {
00043 
00044 namespace network
00045 {
00046 
00047 std::string g_host;
00048 uint16_t g_tcpros_server_port = 0;
00049 
00050 const std::string& getHost()
00051 {
00052   return g_host;
00053 }
00054 
00055 bool splitURI(const std::string& uri, std::string& host, uint32_t& port)
00056 {
00057   // skip over the protocol if it's there
00058   if (uri.substr(0, 7) == std::string("http://"))
00059     host = uri.substr(7);
00060   else if (uri.substr(0, 9) == std::string("rosrpc://"))
00061     host = uri.substr(9);
00062   // split out the port
00063   std::string::size_type colon_pos = host.find_first_of(":");
00064   if (colon_pos == std::string::npos)
00065     return false;
00066   std::string port_str = host.substr(colon_pos+1);
00067   std::string::size_type slash_pos = port_str.find_first_of("/");
00068   if (slash_pos != std::string::npos)
00069     port_str = port_str.erase(slash_pos);
00070   port = atoi(port_str.c_str());
00071   host = host.erase(colon_pos);
00072   return true;
00073 }
00074 
00075 uint16_t getTCPROSPort()
00076 {
00077   return g_tcpros_server_port;
00078 }
00079 
00080 static bool isPrivateIP(const char *ip)
00081 {
00082   bool b = !strncmp("192.168", ip, 7) || !strncmp("10.", ip, 3) ||
00083            !strncmp("169.254", ip, 7);
00084   return b;
00085 }
00086 
00087 std::string determineHost()
00088 {
00089   std::string ip_env;
00090   // First, did the user set ROS_HOSTNAME?
00091   if ( get_environment_variable(ip_env, "ROS_HOSTNAME")) {
00092     ROSCPP_LOG_DEBUG( "determineIP: using value of ROS_HOSTNAME:%s:", ip_env.c_str());
00093     if (ip_env.size() == 0)
00094     {
00095       ROS_WARN("invalid ROS_HOSTNAME (an empty string)");
00096     }
00097     return ip_env;
00098   }
00099 
00100   // Second, did the user set ROS_IP?
00101   if ( get_environment_variable(ip_env, "ROS_IP")) {
00102     ROSCPP_LOG_DEBUG( "determineIP: using value of ROS_IP:%s:", ip_env.c_str());
00103     if (ip_env.size() == 0)
00104     {
00105       ROS_WARN("invalid ROS_IP (an empty string)");
00106     }
00107     return ip_env;
00108   }
00109 
00110   // Third, try the hostname
00111   char host[1024];
00112   memset(host,0,sizeof(host));
00113   if(gethostname(host,sizeof(host)-1) != 0)
00114   {
00115     ROS_ERROR("determineIP: gethostname failed");
00116   }
00117   // We don't want localhost to be our ip
00118   else if(strlen(host) && strcmp("localhost", host))
00119   {
00120     return std::string(host);
00121   }
00122 
00123   // Fourth, fall back on interface search, which will yield an IP address
00124 
00125 #ifdef HAVE_IFADDRS_H
00126   struct ifaddrs *ifa = NULL, *ifp = NULL;
00127   int rc;
00128   if ((rc = getifaddrs(&ifp)) < 0)
00129   {
00130     ROS_FATAL("error in getifaddrs: [%s]", strerror(rc));
00131     ROS_BREAK();
00132   }
00133   char preferred_ip[200] = {0};
00134   for (ifa = ifp; ifa; ifa = ifa->ifa_next)
00135   {
00136     char ip_[200];
00137     socklen_t salen;
00138     if (!ifa->ifa_addr)
00139       continue; // evidently this interface has no ip address
00140     if (ifa->ifa_addr->sa_family == AF_INET)
00141       salen = sizeof(struct sockaddr_in);
00142     else if (ifa->ifa_addr->sa_family == AF_INET6)
00143       salen = sizeof(struct sockaddr_in6);
00144     else
00145       continue;
00146     if (getnameinfo(ifa->ifa_addr, salen, ip_, sizeof(ip_), NULL, 0,
00147                     NI_NUMERICHOST) < 0)
00148     {
00149       ROSCPP_LOG_DEBUG( "getnameinfo couldn't get the ip of interface [%s]", ifa->ifa_name);
00150       continue;
00151     }
00152     //ROS_INFO( "ip of interface [%s] is [%s]", ifa->ifa_name, ip);
00153     // prefer non-private IPs over private IPs
00154     if (!strcmp("127.0.0.1", ip_) || strchr(ip_,':'))
00155       continue; // ignore loopback unless we have no other choice
00156     if (ifa->ifa_addr->sa_family == AF_INET6 && !preferred_ip[0])
00157       strcpy(preferred_ip, ip_);
00158     else if (isPrivateIP(ip_) && !preferred_ip[0])
00159       strcpy(preferred_ip, ip_);
00160     else if (!isPrivateIP(ip_) &&
00161              (isPrivateIP(preferred_ip) || !preferred_ip[0]))
00162       strcpy(preferred_ip, ip_);
00163   }
00164   freeifaddrs(ifp);
00165   if (!preferred_ip[0])
00166   {
00167     ROS_ERROR( "Couldn't find a preferred IP via the getifaddrs() call; I'm assuming that your IP "
00168         "address is 127.0.0.1.  This should work for local processes, "
00169         "but will almost certainly not work if you have remote processes."
00170         "Report to the ROS development team to seek a fix.");
00171     return std::string("127.0.0.1");
00172   }
00173   ROSCPP_LOG_DEBUG( "preferred IP is guessed to be %s", preferred_ip);
00174   return std::string(preferred_ip);
00175 #else
00176   // @todo Fix IP determination in the case where getifaddrs() isn't
00177   // available.
00178   ROS_ERROR( "You don't have the getifaddrs() call; I'm assuming that your IP "
00179              "address is 127.0.0.1.  This should work for local processes, "
00180              "but will almost certainly not work if you have remote processes."
00181              "Report to the ROS development team to seek a fix.");
00182   return std::string("127.0.0.1");
00183 #endif
00184 }
00185 
00186 void init(const M_string& remappings)
00187 {
00188   M_string::const_iterator it = remappings.find("__hostname");
00189   if (it != remappings.end())
00190   {
00191     g_host = it->second;
00192   }
00193   else
00194   {
00195     it = remappings.find("__ip");
00196     if (it != remappings.end())
00197     {
00198       g_host = it->second;
00199     }
00200   }
00201 
00202   it = remappings.find("__tcpros_server_port");
00203   if (it != remappings.end())
00204   {
00205     try
00206     {
00207       g_tcpros_server_port = boost::lexical_cast<uint16_t>(it->second);
00208     }
00209     catch (boost::bad_lexical_cast&)
00210     {
00211       throw ros::InvalidPortException("__tcpros_server_port [" + it->second + "] was not specified as a number within the 0-65535 range");
00212     }
00213   }
00214 
00215   if (g_host.empty())
00216   {
00217     g_host = determineHost();
00218   }
00219 }
00220 
00221 } // namespace network
00222 
00223 } // namespace ros


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Fri Aug 28 2015 12:33:10