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 #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
00082 free(master_uri_env);
00083 #endif
00084 }
00085
00086
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] = "";
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 }
00252
00253 }