foreign_relay.cpp
Go to the documentation of this file.
00001 
00002 // relay just passes messages on. it can be useful if you're trying to ensure
00003 // that a message doesn't get sent twice over a wireless link, by having the 
00004 // relay catch the message and then do the fanout on the far side of the 
00005 // wireless link.
00006 //
00007 // Copyright (C) 2009, Morgan Quigley
00008 //
00009 // Redistribution and use in source and binary forms, with or without
00010 // modification, are permitted provided that the following conditions are met:
00011 //   * Redistributions of source code must retain the above copyright notice,
00012 //     this list of conditions and the following disclaimer.
00013 //   * Redistributions in binary form must reproduce the above copyright
00014 //     notice, this list of conditions and the following disclaimer in the
00015 //     documentation and/or other materials provided with the distribution.
00016 //   * Neither the name of Stanford University nor the names of its
00017 //     contributors may be used to endorse or promote products derived from
00018 //     this software without specific prior written permission.
00019 //
00020 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00021 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00022 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00023 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00024 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00025 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00026 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00027 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00028 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00029 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00030 // POSSIBILITY OF SUCH DAMAGE.
00032 
00033 
00034 #include <cstdio>
00035 #include "topic_tools/shape_shifter.h"
00036 #include "topic_tools/parse.h"
00037 #include "XmlRpc.h"
00038 #include "ros/xmlrpc_manager.h"
00039 #include "ros/network.h"
00040 
00041 using std::string;
00042 using std::vector;
00043 using namespace topic_tools;
00044 
00045 static ros::NodeHandle *g_node = NULL;
00046 static bool g_advertised = false;
00047 static string g_foreign_topic;
00048 static string g_local_topic;
00049 static ros::Publisher g_pub;
00050 static string g_host;
00051 static uint32_t g_port = 0;
00052 static bool g_error = false;
00053 typedef enum
00054 {
00055   MODE_ADV,
00056   MODE_SUB
00057 } relay_mode_t;
00058 static relay_mode_t g_mode;
00059 
00060 #define USAGE "USAGE: foreign_relay {adv|sub} FOREIGN_MASTER_URI FOREIGN_TOPIC LOCAL_TOPIC"
00061 
00062 ros::XMLRPCManagerPtr g_xmlrpc_manager = ros::XMLRPCManager::instance();
00063 
00064 void foreign_advertise(const std::string &type);
00065 void foreign_unadvertise();
00066 void foreign_subscribe();
00067 void foreign_unsubscribe();
00068 void in_cb(const boost::shared_ptr<ShapeShifter const>& msg);
00069 
00070 void foreign_advertise(const std::string &type)
00071 {
00072   XmlRpc::XmlRpcClient *client = g_xmlrpc_manager->getXMLRPCClient(g_host, g_port, "/");
00073   XmlRpc::XmlRpcValue args, result;
00074   args[0] = ros::this_node::getName();
00075   args[1] = g_foreign_topic;
00076   args[2] = type;
00077   args[3] = g_xmlrpc_manager->getServerURI();
00078   if (!client->execute("registerPublisher", args, result))
00079   {
00080     ROS_FATAL("Failed to contact foreign master at [%s:%d] to register [%s].", g_host.c_str(), g_port, g_foreign_topic.c_str());
00081     g_error = true;
00082     ros::shutdown();
00083   }
00084   ros::XMLRPCManager::instance()->releaseXMLRPCClient(client);
00085 }
00086 
00087 void foreign_unadvertise()
00088 {
00089   XmlRpc::XmlRpcClient *client = g_xmlrpc_manager->getXMLRPCClient(g_host, g_port, "/");
00090   XmlRpc::XmlRpcValue args, result;
00091   args[0] = ros::this_node::getName();
00092   args[1] = g_foreign_topic;
00093   args[2] = g_xmlrpc_manager->getServerURI();
00094   if (!client->execute("unregisterPublisher", args, result))
00095   {
00096     ROS_ERROR("Failed to contact foreign master at [%s:%d] to unregister [%s].", g_host.c_str(), g_port, g_foreign_topic.c_str());
00097     g_error = true;
00098   }
00099   ros::XMLRPCManager::instance()->releaseXMLRPCClient(client);
00100 }
00101 
00102 void foreign_subscribe()
00103 {
00104   XmlRpc::XmlRpcClient *client = 
00105           g_xmlrpc_manager->getXMLRPCClient(g_host, g_port, "/");
00106   XmlRpc::XmlRpcValue args, result, payload;
00107   args[0] = ros::this_node::getName();
00108   args[1] = g_foreign_topic;
00109   args[2] = "*";
00110   args[3] = g_xmlrpc_manager->getServerURI();
00111   if (!client->execute("registerSubscriber", args, result))
00112   {
00113     ROS_FATAL("Failed to contact foreign master at [%s:%d] to register [%s].", 
00114               g_host.c_str(), g_port, g_foreign_topic.c_str());
00115     g_error = true;
00116     ros::shutdown();
00117     return;
00118   }
00119   
00120   {
00121     // Horrible hack: the response from registerSubscriber() can contain a
00122     // list of current publishers.  But we don't have a way of injecting them
00123     // into roscpp here.  Now, if we get a publisherUpdate() from the master,
00124     // everything will work.  So, we ask the master if anyone is currently
00125     // publishing the topic, grab the advertised type, use it to advertise
00126     // ourselves, then unadvertise, triggering a publisherUpdate() along the
00127     // way.
00128     XmlRpc::XmlRpcValue args, result, payload;
00129     args[0] = ros::this_node::getName();
00130     args[1] = std::string("");
00131     if(!client->execute("getPublishedTopics", args, result))
00132     {
00133       ROS_FATAL("Failed to call getPublishedTopics() on foreign master at [%s:%d]",
00134                 g_host.c_str(), g_port);
00135       g_error = true;
00136       ros::shutdown();
00137       return;
00138     }
00139     if (!ros::XMLRPCManager::instance()->validateXmlrpcResponse("getPublishedTopics", result, payload))
00140     {
00141       ROS_FATAL("Failed to get validate response to getPublishedTopics() from foreign master at [%s:%d]",
00142                 g_host.c_str(), g_port);
00143       g_error = true;
00144       ros::shutdown();
00145       return;
00146     }
00147     for(int i=0;i<payload.size();i++)
00148     {
00149       std::string topic = std::string(payload[i][0]);
00150       std::string type = std::string(payload[i][1]);
00151 
00152       if(topic == g_foreign_topic)
00153       {
00154         foreign_advertise(type);
00155         foreign_unadvertise();
00156         break;
00157       }
00158     }
00159   }
00160 
00161   ros::XMLRPCManager::instance()->releaseXMLRPCClient(client);
00162 }
00163 
00164 void foreign_unsubscribe()
00165 {
00166   XmlRpc::XmlRpcClient *client = g_xmlrpc_manager->getXMLRPCClient(g_host, g_port, "/");
00167   XmlRpc::XmlRpcValue args, result;
00168   args[0] = ros::this_node::getName();
00169   args[1] = g_foreign_topic;
00170   args[2] = g_xmlrpc_manager->getServerURI();
00171   if (!client->execute("unregisterSubscriber", args, result))
00172   {
00173     ROS_ERROR("Failed to contact foreign master at [%s:%d] to unregister [%s].", g_host.c_str(), g_port, g_foreign_topic.c_str());
00174     g_error = true;
00175   }
00176   ros::XMLRPCManager::instance()->releaseXMLRPCClient(client);
00177 }
00178 
00179 void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
00180 {
00181   if (!g_advertised)
00182   {
00183     ROS_INFO("Received message from: %s",
00184              (*(msg->__connection_header))["callerid"].c_str());
00185     if(g_mode == MODE_SUB)
00186     {
00187       // Advertise locally
00188       g_pub = msg->advertise(*g_node, g_local_topic, 10);
00189       ROS_INFO("Advertised locally as %s, with type %s", 
00190                g_local_topic.c_str(),
00191                (*(msg->__connection_header))["type"].c_str());
00192     }
00193     else
00194     {
00195       // We advertise locally as a hack, to get things set up properly.
00196       g_pub = msg->advertise(*g_node, g_foreign_topic, 10);
00197       // Advertise at the foreign master.
00198       foreign_advertise((*(msg->__connection_header))["type"]);
00199       ROS_INFO("Advertised foreign as %s, with type %s", 
00200                g_foreign_topic.c_str(),
00201                (*(msg->__connection_header))["type"].c_str());
00202     }
00203     g_advertised = true;
00204   }
00205   g_pub.publish(msg);
00206 }
00207 
00208 int main(int argc, char **argv)
00209 {
00210   if (argc < 5)
00211   {
00212     ROS_FATAL(USAGE);
00213     return 1;
00214   }
00215   if(std::string(argv[1]) == "adv")
00216     g_mode = MODE_ADV;
00217   else if(std::string(argv[1]) == "sub")
00218     g_mode = MODE_SUB;
00219   else
00220   {
00221     ROS_FATAL(USAGE);
00222     return 1;
00223   }
00224   std::string foreign_master_uri;
00225   foreign_master_uri = argv[2];
00226   g_foreign_topic = argv[3];
00227   g_local_topic = argv[4];
00228   std::string local_topic_basename;
00229   if(!getBaseName(g_local_topic, local_topic_basename))
00230   {
00231     ROS_FATAL("Failed to extract basename from topic [%s]", 
00232               g_local_topic.c_str());
00233     return 1;
00234   }
00235   if (!ros::network::splitURI(foreign_master_uri, g_host, g_port))
00236   {
00237     ROS_FATAL("Couldn't parse the foreign master URI [%s] into a host:port pair.", foreign_master_uri.c_str());
00238     return 1;
00239   }
00240 
00241   char buf[1024];
00242   snprintf(buf, sizeof(buf), "%d", ros::master::getPort());
00243   ros::init(argc, argv, local_topic_basename + string("_foreign_relay_") + buf + "_" + ((g_mode == MODE_ADV) ? "adv" : "sub"),
00244             ros::init_options::AnonymousName);
00245   ros::NodeHandle n;
00246   ros::NodeHandle pnh("~");
00247   g_node = &pnh;
00248 
00249   ros::Subscriber sub;
00250   if(g_mode == MODE_SUB)
00251   {
00252     // We subscribe locally as a hack, to get our callback set up properly.
00253     sub = n.subscribe<ShapeShifter>(g_foreign_topic, 10, &in_cb, 
00254                                     ros::TransportHints().unreliable());
00255     // Subscribe at foreign master.
00256     foreign_subscribe();
00257   }
00258   else
00259   {
00260     // Subscribe at local master.
00261     sub = n.subscribe<ShapeShifter>(g_local_topic, 10, &in_cb);
00262   }
00263 
00264   ros::spin();
00265 
00266   if(g_mode == MODE_SUB)
00267     foreign_unsubscribe();
00268   else
00269     foreign_unadvertise();
00270 
00271   return g_error;
00272 }
00273 


foreign_relay
Author(s): Blaise Gassend
autogenerated on Mon Dec 2 2013 11:36:50