master.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 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 "ros/master.h"
00029 #include "ros/xmlrpc_manager.h"
00030 #include "ros/this_node.h"
00031 #include "ros/init.h"
00032 #include "ros/network.h"
00033 
00034 #include <ros/console.h>
00035 #include <ros/assert.h>
00036 
00037 #include "XmlRpc.h"
00038 
00039 namespace ros
00040 {
00041 
00042 namespace master
00043 {
00044 
00045 uint32_t g_port = 0;
00046 std::string g_host;
00047 std::string g_uri;
00048 ros::WallDuration g_retry_timeout;
00049 
00050 void init(const M_string& remappings)
00051 {
00052   M_string::const_iterator it = remappings.find("__master");
00053   if (it != remappings.end())
00054   {
00055     g_uri = it->second;
00056   }
00057 
00058   if (g_uri.empty())
00059   {
00060     char *master_uri_env = NULL;
00061     #ifdef _MSC_VER
00062       _dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
00063     #else
00064       master_uri_env = getenv("ROS_MASTER_URI");
00065     #endif
00066     if (!master_uri_env)
00067     {
00068       ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
00069                  "type the following or (preferrably) add this to your " \
00070                  "~/.bashrc file in order set up your " \
00071                  "local machine as a ROS master:\n\n" \
00072                  "export ROS_MASTER_URI=http://localhost:11311\n\n" \
00073                  "then, type 'roscore' in another shell to actually launch " \
00074                  "the master program.");
00075       ROS_BREAK();
00076     }
00077 
00078     g_uri = master_uri_env;
00079 
00080 #ifdef _MSC_VER
00081     // http://msdn.microsoft.com/en-us/library/ms175774(v=vs.80).aspx
00082     free(master_uri_env);
00083 #endif
00084   }
00085 
00086   // Split URI into
00087   if (!network::splitURI(g_uri, g_host, g_port))
00088   {
00089     ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
00090     ROS_BREAK();
00091   }
00092 }
00093 
00094 const std::string& getHost()
00095 {
00096   return g_host;
00097 }
00098 
00099 uint32_t getPort()
00100 {
00101   return g_port;
00102 }
00103 
00104 const std::string& getURI()
00105 {
00106   return g_uri;
00107 }
00108 
00109 void setRetryTimeout(ros::WallDuration timeout)
00110 {
00111   if (timeout < ros::WallDuration(0))
00112   {
00113     ROS_FATAL("retry timeout must not be negative.");
00114     ROS_BREAK();
00115   }
00116   g_retry_timeout = timeout;
00117 }
00118 
00119 bool check()
00120 {
00121   XmlRpc::XmlRpcValue args, result, payload;
00122   args[0] = this_node::getName();
00123   return execute("getPid", args, result, payload, false);
00124 }
00125 
00126 bool getTopics(V_TopicInfo& topics)
00127 {
00128   XmlRpc::XmlRpcValue args, result, payload;
00129   args[0] = this_node::getName();
00130   args[1] = ""; //TODO: Fix this
00131 
00132   if (!execute("getPublishedTopics", args, result, payload, true))
00133   {
00134     return false;
00135   }
00136 
00137   topics.clear();
00138   for (int i = 0; i < payload.size(); i++)
00139   {
00140     topics.push_back(TopicInfo(std::string(payload[i][0]), std::string(payload[i][1])));
00141   }
00142 
00143   return true;
00144 }
00145 
00146 bool getNodes(V_string& nodes)
00147 {
00148   XmlRpc::XmlRpcValue args, result, payload;
00149   args[0] = this_node::getName();
00150 
00151   if (!execute("getSystemState", args, result, payload, true))
00152   {
00153     return false;
00154   }
00155 
00156   S_string node_set;
00157   for (int i = 0; i < payload.size(); ++i)
00158   {
00159     for (int j = 0; j < payload[i].size(); ++j)
00160     {
00161       XmlRpc::XmlRpcValue val = payload[i][j][1];
00162       for (int k = 0; k < val.size(); ++k)
00163       {
00164         std::string name = payload[i][j][1][k];
00165         node_set.insert(name);
00166       }
00167     }
00168   }
00169 
00170   nodes.insert(nodes.end(), node_set.begin(), node_set.end());
00171 
00172   return true;
00173 }
00174 
00175 #if defined(__APPLE__)
00176 boost::mutex g_xmlrpc_call_mutex;
00177 #endif
00178 
00179 bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master)
00180 {
00181   ros::WallTime start_time = ros::WallTime::now();
00182 
00183   std::string master_host = getHost();
00184   uint32_t master_port = getPort();
00185   XmlRpc::XmlRpcClient *c = XMLRPCManager::instance()->getXMLRPCClient(master_host, master_port, "/");
00186   bool printed = false;
00187   bool slept = false;
00188   bool ok = true;
00189   do
00190   {
00191     bool b = false;
00192     {
00193 #if defined(__APPLE__)
00194       boost::mutex::scoped_lock lock(g_xmlrpc_call_mutex);
00195 #endif
00196 
00197       b = c->execute(method.c_str(), request, response);
00198     }
00199 
00200     ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
00201 
00202     if (!b && ok)
00203     {
00204       if (!printed && wait_for_master)
00205       {
00206         ROS_ERROR("[%s] Failed to contact master at [%s:%d].  %s", method.c_str(), master_host.c_str(), master_port, wait_for_master ? "Retrying..." : "");
00207         printed = true;
00208       }
00209 
00210       if (!wait_for_master)
00211       {
00212         XMLRPCManager::instance()->releaseXMLRPCClient(c);
00213         return false;
00214       }
00215 
00216       if (!g_retry_timeout.isZero() && (ros::WallTime::now() - start_time) >= g_retry_timeout)
00217       {
00218         ROS_ERROR("[%s] Timed out trying to connect to the master after [%f] seconds", method.c_str(), g_retry_timeout.toSec());
00219         XMLRPCManager::instance()->releaseXMLRPCClient(c);
00220         return false;
00221       }
00222 
00223       ros::WallDuration(0.05).sleep();
00224       slept = true;
00225     }
00226     else
00227     {
00228       if (!XMLRPCManager::instance()->validateXmlrpcResponse(method, response, payload))
00229       {
00230         XMLRPCManager::instance()->releaseXMLRPCClient(c);
00231 
00232         return false;
00233       }
00234 
00235       break;
00236     }
00237 
00238     ok = !ros::isShuttingDown() && !XMLRPCManager::instance()->isShuttingDown();
00239   } while(ok);
00240 
00241   if (ok && slept)
00242   {
00243     ROS_INFO("Connected to master at [%s:%d]", master_host.c_str(), master_port);
00244   }
00245 
00246   XMLRPCManager::instance()->releaseXMLRPCClient(c);
00247 
00248   return true;
00249 }
00250 
00251 } // namespace master
00252 
00253 } // namespace ros


roscpp
Author(s): Morgan Quigley, Josh Faust, Brian Gerkey, Troy Straszheim
autogenerated on Tue Mar 7 2017 04:01:03