v01

thirdparty/ros/ros_comm/tools/topic_tools/src/demux.cpp

@@ -0,0 +1,312 @@
+///////////////////////////////////////////////////////////////////////////////
+// demux is a generic ROS topic demultiplexer: one input topic is fanned out
+// to 1 of N output topics. A service is provided to select between the outputs
+//
+// Copyright (C) 2009, Morgan Quigley
+// Copyright (C) 2014, Andreas Hermann
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Stanford University nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+/////////////////////////////////////////////////////////////////////////////
+
+
+#include <cstdio>
+#include <vector>
+#include <list>
+#include "ros/console.h"
+#include "std_msgs/String.h"
+#include "topic_tools/DemuxSelect.h"
+#include "topic_tools/DemuxAdd.h"
+#include "topic_tools/DemuxList.h"
+#include "topic_tools/DemuxDelete.h"
+#include "topic_tools/shape_shifter.h"
+#include "topic_tools/parse.h"
+
+using std::string;
+using std::vector;
+using std::list;
+using namespace topic_tools;
+
+const static string g_none_topic = "__none";
+
+static ros::NodeHandle *g_node = NULL;
+
+static string g_input_topic;
+static ros::Subscriber g_sub; // the input toppic
+static ros::Publisher g_pub_selected; // publishes name of selected publisher toppic
+
+struct pub_info_t
+{
+  std::string topic_name;
+  ros::Publisher *pub;
+};
+
+void in_cb(const boost::shared_ptr<ShapeShifter const>& msg);
+
+static list<struct pub_info_t> g_pubs; // the list of publishers
+static list<struct pub_info_t>::iterator g_selected = g_pubs.end();
+
+bool sel_srv_cb( topic_tools::DemuxSelect::Request  &req,
+                 topic_tools::DemuxSelect::Response &res )
+{
+  bool ret = false;
+  if (g_selected != g_pubs.end()) {
+    res.prev_topic = g_selected->topic_name;
+  }
+  else
+    res.prev_topic = string("");
+
+  // see if it's the magical '__none' topic, in which case we open the circuit
+  if (req.topic == g_none_topic)
+  {
+    ROS_INFO("demux selected to no output.");
+
+    g_selected = g_pubs.end();
+    ret = true;
+  }
+  else
+  {
+    ROS_INFO("trying to switch demux to %s", req.topic.c_str());
+    // spin through our vector of inputs and find this guy
+    for (list<struct pub_info_t>::iterator it = g_pubs.begin();
+	 it != g_pubs.end();
+	 ++it)
+    {
+      if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
+      {
+        g_selected = it;
+        ROS_INFO("demux selected output: [%s]", it->topic_name.c_str());
+        ret = true;
+        break;
+      }
+    }
+    if(!ret)
+    {
+    	ROS_WARN("Failed to switch to non-existing topic %s in demux.", req.topic.c_str());
+    }
+  }
+
+  if(ret)
+  {
+    std_msgs::String t;
+    t.data = req.topic;
+    g_pub_selected.publish(t);
+  }
+
+  return ret;
+}
+
+void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
+{
+  ROS_DEBUG("Received an incoming msg ...");
+  // when a message is incoming, check, if the requested publisher is already existing.
+  // if not, create it with the information available from the incoming message.
+  bool selected_added = false;
+  for (list<struct pub_info_t>::iterator it = g_pubs.begin(); it != g_pubs.end(); ++it) {
+    if (!it->pub)
+    {
+      if (it->topic_name == g_selected->topic_name)
+      {
+        selected_added = true;
+      }
+
+      try
+      {
+        it->pub = new ros::Publisher(msg->advertise(*g_node, it->topic_name, 10, false));
+      }
+      catch (ros::InvalidNameException& e)
+      {
+        ROS_WARN("failed to add topic '%s' to demux, because it's an invalid name: %s",
+                 it->topic_name.c_str(), e.what());
+        return;
+      }
+
+      ROS_INFO("Added publisher %s to demux!", it->topic_name.c_str());
+    }
+  }
+
+  if (selected_added)
+  {
+    // This is needed, because it takes some time before publisher is registered and can send out messages.
+    ROS_INFO("Sleeping 0.5 sec.");
+    ros::Duration(0.5).sleep();
+  }
+
+  // check that we have a valid topic
+  if (!g_selected->pub) return;
+
+  // finally: send out the message over the active publisher
+  g_selected->pub->publish(msg);
+  ROS_DEBUG("... and sent it out again!");
+}
+
+bool list_topic_cb(topic_tools::DemuxList::Request& req,
+	 	   topic_tools::DemuxList::Response& res)
+{
+  (void)req;
+  for (list<struct pub_info_t>::iterator it = g_pubs.begin();
+       it != g_pubs.end();
+       ++it)
+  {
+    res.topics.push_back(it->topic_name);
+  }
+
+  return true;
+}
+
+bool add_topic_cb(topic_tools::DemuxAdd::Request& req,
+		  topic_tools::DemuxAdd::Response& res)
+{
+  (void)res;
+  // Check that it's not already in our list
+  ROS_INFO("trying to add %s to demux", req.topic.c_str());
+
+  // Can't add the __none topic
+  if(req.topic == g_none_topic)
+  {
+    ROS_WARN("failed to add topic %s to demux, because it's reserved for special use",
+	     req.topic.c_str());
+    return false;
+  }
+
+  // spin through our vector of inputs and find this guy
+  for (list<struct pub_info_t>::iterator it = g_pubs.begin();
+       it != g_pubs.end();
+       ++it)
+  {
+    if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
+    {
+      ROS_WARN("tried to add a topic that demux was already publishing: [%s]",
+	       it->topic_name.c_str());
+      return false;
+    }
+  }
+
+  struct pub_info_t pub_info;
+  pub_info.topic_name = ros::names::resolve(req.topic);
+  pub_info.pub = NULL;
+  g_pubs.push_back(pub_info);
+
+  ROS_INFO("PRE added %s to demux", req.topic.c_str());
+
+  return true;
+}
+
+bool del_topic_cb(topic_tools::DemuxDelete::Request& req,
+		  topic_tools::DemuxDelete::Response& res)
+{
+  (void)res;
+  // Check that it's in our list
+  ROS_INFO("trying to delete %s from demux", req.topic.c_str());
+  // spin through our vector of inputs and find this guy
+  for (list<struct pub_info_t>::iterator it = g_pubs.begin();
+       it != g_pubs.end();
+       ++it)
+  {
+    if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
+    {
+      // Can't delete the currently selected input, #2863
+      if(it == g_selected)
+      {
+        ROS_WARN("tried to delete currently selected topic %s from demux", req.topic.c_str());
+        return false;
+      }
+      if (it->pub)
+        it->pub->shutdown();
+      delete it->pub;
+      g_pubs.erase(it);
+      ROS_INFO("deleted topic %s from demux", req.topic.c_str());
+      return true;
+    }
+  }
+
+  ROS_WARN("tried to delete non-published topic %s from demux", req.topic.c_str());
+  return false;
+}
+
+int main(int argc, char **argv)
+{
+  vector<string> args;
+  ros::removeROSArgs(argc, (const char**)argv, args);
+
+  if (args.size() < 3)
+  {
+    printf("\nusage: demux IN_TOPIC OUT_TOPIC1 [OUT_TOPIC2 [...]]\n\n");
+    return 1;
+  }
+  std::string topic_name;
+  if(!getBaseName(args[1], topic_name))
+    return 1;
+  ros::init(argc, argv, topic_name + string("_demux"),
+            ros::init_options::AnonymousName);
+  vector<string> topics;
+  for (unsigned int i = 2; i < args.size(); i++)
+    topics.push_back(args[i]);
+  ros::NodeHandle n;
+  g_node = &n;
+  g_input_topic = args[1];
+  // Put our API into the "demux" namespace, which the user should usually remap
+  ros::NodeHandle demux_nh("demux"), pnh("~");
+
+  // Latched publisher for selected output topic name
+  g_pub_selected = demux_nh.advertise<std_msgs::String>(string("selected"), 1, true);
+
+  for (size_t i = 0; i < topics.size(); i++)
+  {
+    struct pub_info_t pub_info;
+    pub_info.topic_name = ros::names::resolve(topics[i]);
+    pub_info.pub = NULL;
+    g_pubs.push_back(pub_info);
+    ROS_INFO("PRE added %s to demux", topics[i].c_str());
+  }
+  g_selected = g_pubs.begin(); // select first topic to start
+  std_msgs::String t;
+  t.data = g_selected->topic_name;
+  g_pub_selected.publish(t);
+
+  // Create the one subscriber
+  g_sub = ros::Subscriber(n.subscribe<ShapeShifter>(g_input_topic, 10, boost::bind(in_cb, _1)));
+
+
+  // New service
+  ros::ServiceServer ss_select = demux_nh.advertiseService(string("select"), sel_srv_cb);
+  ros::ServiceServer ss_add = demux_nh.advertiseService(string("add"), add_topic_cb);
+  ros::ServiceServer ss_list = demux_nh.advertiseService(string("list"), list_topic_cb);
+  ros::ServiceServer ss_del = demux_nh.advertiseService(string("delete"), del_topic_cb);
+
+  // Run
+  ros::spin();
+
+  // Destruction
+  for (list<struct pub_info_t>::iterator it = g_pubs.begin();
+       it != g_pubs.end();
+       ++it)
+  {
+    if (it->pub)
+      it->pub->shutdown();
+    delete it->pub;
+  }
+
+  g_pubs.clear();
+  return 0;
+}

thirdparty/ros/ros_comm/tools/topic_tools/src/drop.cpp

@@ -0,0 +1,93 @@
+///////////////////////////////////////////////////////////////////////////////
+// drop will (intentionally) drop X out of every Y messages that hits it
+//
+// Copyright (C) 2009, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Stanford University nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+/////////////////////////////////////////////////////////////////////////////
+
+#include <cstdlib>
+#include <cstdio>
+#include "topic_tools/shape_shifter.h"
+#include "topic_tools/parse.h"
+
+using std::string;
+using std::vector;
+using namespace topic_tools;
+
+static ros::NodeHandle *g_node = NULL;
+static int g_x = 0, g_y = 1;
+static bool g_advertised = false;
+static string g_output_topic;
+static ros::Publisher g_pub;
+
+void in_cb(const boost::shared_ptr<ShapeShifter const>& msg)
+{
+  static int s_count = 0;
+  if (!g_advertised)
+  {
+    g_pub = msg->advertise(*g_node, g_output_topic, 10);
+    g_advertised = true;
+    printf("advertised as %s\n", g_output_topic.c_str());
+  }
+  if (s_count >= g_x)
+    g_pub.publish(msg);
+  ++s_count;
+  if (s_count >= g_y)
+    s_count = 0;
+}
+
+#define USAGE "\nusage: drop IN_TOPIC X Y [OUT_TOPIC]\n\n" \
+              " This program will drop X out of every Y messages from IN_TOPIC,\n" \
+              " forwarding the rest to OUT_TOPIC if given, else to a topic \n" \
+              " named IN_TOPIC_drop\n\n"
+int main(int argc, char **argv)
+{
+  if(argc < 2)
+  {
+    puts(USAGE);
+    return 1;
+  }
+  std::string topic_name;
+  if(!getBaseName(string(argv[1]), topic_name))
+    return 1;
+  ros::init(argc, argv, topic_name + string("_drop"),
+            ros::init_options::AnonymousName);
+  if ((argc != 4 && argc != 5) || atoi(argv[2]) < 0 || atoi(argv[3]) < 1)
+  {
+    puts(USAGE);
+    return 1;
+  }
+  if (argc == 4)
+    g_output_topic = string(argv[1]) + string("_drop");
+  else // argc == 5
+    g_output_topic = string(argv[4]);
+  ros::NodeHandle n;
+  g_node = &n;
+  g_x = atoi(argv[2]);
+  g_y = atoi(argv[3]);
+  ros::Subscriber sub = n.subscribe<ShapeShifter>(string(argv[1]), 10, in_cb);
+  ros::spin();
+  return 0;
+}

thirdparty/ros/ros_comm/tools/topic_tools/src/mux.cpp

@@ -0,0 +1,346 @@
+///////////////////////////////////////////////////////////////////////////////
+// demux is a generic ROS topic demultiplexer: one input topic is fanned out
+// to 1 of N output topics. A service is provided to select between the outputs
+//
+// Copyright (C) 2009, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Stanford University nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+/////////////////////////////////////////////////////////////////////////////
+
+
+#include <cstdio>
+#include <vector>
+#include <list>
+#include "ros/console.h"
+#include "std_msgs/String.h"
+#include "topic_tools/MuxSelect.h"
+#include "topic_tools/MuxAdd.h"
+#include "topic_tools/MuxList.h"
+#include "topic_tools/MuxDelete.h"
+#include "topic_tools/shape_shifter.h"
+#include "topic_tools/parse.h"
+
+using std::string;
+using std::vector;
+using std::list;
+using namespace topic_tools;
+
+const static string g_none_topic = "__none";
+
+static ros::NodeHandle *g_node = NULL;
+static bool g_lazy = false;
+static bool g_advertised = false;
+static string g_output_topic;
+static ros::Publisher g_pub;
+static ros::Publisher g_pub_selected;
+
+struct sub_info_t
+{
+  std::string topic_name;
+  ros::Subscriber *sub;
+  ShapeShifter* msg;
+};
+
+void in_cb(const boost::shared_ptr<ShapeShifter const>& msg, ShapeShifter* s);
+
+static list<struct sub_info_t> g_subs;
+static list<struct sub_info_t>::iterator g_selected = g_subs.end();
+
+void conn_cb(const ros::SingleSubscriberPublisher&)
+{
+  // If we're in lazy subscribe mode, and the first subscriber just
+  // connected, then subscribe
+  if(g_lazy && g_selected != g_subs.end() && !g_selected->sub)
+  {
+    ROS_DEBUG("lazy mode; resubscribing to %s", g_selected->topic_name.c_str());
+    g_selected->sub = new ros::Subscriber(g_node->subscribe<ShapeShifter>(g_selected->topic_name, 10, boost::bind(in_cb, _1, g_selected->msg)));
+  }
+}
+
+bool sel_srv_cb( topic_tools::MuxSelect::Request  &req,
+                 topic_tools::MuxSelect::Response &res )
+{
+  bool ret = false;
+  if (g_selected != g_subs.end()) {
+    res.prev_topic = g_selected->topic_name;
+
+    // Unsubscribe to old topic if lazy
+    if (g_lazy) {
+      ROS_DEBUG("Unsubscribing to %s, lazy", res.prev_topic.c_str());
+      if (g_selected->sub)
+        g_selected->sub->shutdown();
+      delete g_selected->sub;
+      g_selected->sub = NULL;
+    }
+  }
+  else
+    res.prev_topic = string("");
+
+  // see if it's the magical '__none' topic, in which case we open the circuit
+  if (req.topic == g_none_topic)
+  {
+    ROS_INFO("mux selected to no input.");
+
+    g_selected = g_subs.end();
+    ret = true;
+  }
+  else
+  {
+    ROS_INFO("trying to switch mux to %s", req.topic.c_str());
+    // spin through our vector of inputs and find this guy
+    for (list<struct sub_info_t>::iterator it = g_subs.begin();
+	 it != g_subs.end();
+	 ++it)
+    {
+      if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
+      {
+        g_selected = it;
+        ROS_INFO("mux selected input: [%s]", it->topic_name.c_str());
+        ret = true;
+        
+        if (!g_selected->sub && (!g_advertised || (g_advertised && g_pub.getNumSubscribers()))) {
+          g_selected->sub = new ros::Subscriber(g_node->subscribe<ShapeShifter>(g_selected->topic_name, 10, boost::bind(in_cb, _1, g_selected->msg)));
+        }
+      }
+    }
+  }
+  
+  if(ret)
+  {
+    std_msgs::String t;
+    t.data = req.topic;
+    g_pub_selected.publish(t);
+  }
+
+  return ret;
+}
+
+bool sel_srv_cb_dep( topic_tools::MuxSelect::Request  &req,
+		     topic_tools::MuxSelect::Response &res )
+{
+  ROS_WARN("the <topic>_select service is deprecated; use mux/select instead");
+  return sel_srv_cb(req,res);
+}
+
+
+void in_cb(const boost::shared_ptr<ShapeShifter const>& msg,
+           ShapeShifter* s)
+{
+  if (!g_advertised)
+  {
+    ROS_INFO("advertising");
+    g_pub = msg->advertise(*g_node, g_output_topic, 10, false, conn_cb);
+    g_advertised = true;
+    
+    // If lazy, unregister from all but the selected topic
+    if (g_lazy) {
+      for (static list<struct sub_info_t>::iterator it = g_subs.begin(); it != g_subs.end(); ++it) {
+        if (it != g_selected) {
+          ROS_INFO("Unregistering from %s", it->topic_name.c_str());
+          if (it->sub)
+            it->sub->shutdown();
+          delete it->sub;
+          it->sub = NULL;
+        }
+	    	}
+    }
+  }
+  
+  if (s != g_selected->msg)
+    return;
+  
+  // If we're in lazy subscribe mode, and nobody's listening, then unsubscribe
+  if (g_lazy && !g_pub.getNumSubscribers() && g_selected != g_subs.end()) {
+    ROS_INFO("lazy mode; unsubscribing");
+    g_selected->sub->shutdown();
+    delete g_selected->sub;
+    g_selected->sub = NULL;
+  }
+  else
+    g_pub.publish(msg);
+}
+
+bool list_topic_cb(topic_tools::MuxList::Request& req,
+	 	   topic_tools::MuxList::Response& res)
+{
+  (void)req;
+  for (list<struct sub_info_t>::iterator it = g_subs.begin();
+       it != g_subs.end();
+       ++it)
+  {
+    res.topics.push_back(it->topic_name);
+  }
+
+  return true;
+}
+
+bool add_topic_cb(topic_tools::MuxAdd::Request& req,
+		  topic_tools::MuxAdd::Response& res)
+{
+  (void)res;
+  // Check that it's not already in our list
+  ROS_INFO("trying to add %s to mux", req.topic.c_str());
+  
+  // Can't add the __none topic
+  if(req.topic == g_none_topic)
+  {
+    ROS_WARN("failed to add topic %s to mux, because it's reserved for special use",
+	     req.topic.c_str());
+    return false;
+  }
+
+  // spin through our vector of inputs and find this guy
+  for (list<struct sub_info_t>::iterator it = g_subs.begin();
+       it != g_subs.end();
+       ++it)
+  {
+    if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
+    {
+      ROS_WARN("tried to add a topic that mux was already listening to: [%s]", 
+	       it->topic_name.c_str());
+      return false;
+    }
+  }
+
+  struct sub_info_t sub_info;
+  sub_info.msg = new ShapeShifter;
+  sub_info.topic_name = ros::names::resolve(req.topic);
+  try
+  {
+    if (g_lazy)
+      sub_info.sub = NULL;
+    else
+      sub_info.sub = new ros::Subscriber(g_node->subscribe<ShapeShifter>(sub_info.topic_name, 10, boost::bind(in_cb, _1, sub_info.msg)));
+  }
+  catch(ros::InvalidNameException& e)
+  {
+    ROS_WARN("failed to add topic %s to mux, because it's an invalid name: %s",
+	     req.topic.c_str(), e.what());
+    delete sub_info.msg;
+    return false;
+  }
+  g_subs.push_back(sub_info);
+
+  ROS_INFO("added %s to mux", req.topic.c_str());
+
+  return true;
+}
+
+bool del_topic_cb(topic_tools::MuxDelete::Request& req,
+		  topic_tools::MuxDelete::Response& res)
+{
+  (void)res;
+  // Check that it's in our list
+  ROS_INFO("trying to delete %s from mux", req.topic.c_str());
+  // spin through our vector of inputs and find this guy
+  for (list<struct sub_info_t>::iterator it = g_subs.begin();
+       it != g_subs.end();
+       ++it)
+  {
+    if (ros::names::resolve(it->topic_name) == ros::names::resolve(req.topic))
+    {
+      // Can't delete the currently selected input, #2863
+      if(it == g_selected)
+      {
+        ROS_WARN("tried to delete currently selected topic %s from mux", req.topic.c_str());
+        return false;
+      }
+      if (it->sub)
+        it->sub->shutdown();
+      delete it->sub;
+      delete it->msg;
+      g_subs.erase(it);
+      ROS_INFO("deleted topic %s from mux", req.topic.c_str());
+      return true;
+    }
+  }
+
+  ROS_WARN("tried to delete non-subscribed topic %s from mux", req.topic.c_str());
+  return false;
+}
+
+int main(int argc, char **argv)
+{
+  vector<string> args;
+  ros::removeROSArgs(argc, (const char**)argv, args);
+
+  if (args.size() < 3)
+  {
+    printf("\nusage: mux OUT_TOPIC IN_TOPIC1 [IN_TOPIC2 [...]]\n\n");
+    return 1;
+  }
+  std::string topic_name;
+  if(!getBaseName(args[1], topic_name))
+    return 1;
+  ros::init(argc, argv, topic_name + string("_mux"),
+            ros::init_options::AnonymousName);
+  vector<string> topics;
+  for (unsigned int i = 2; i < args.size(); i++)
+    topics.push_back(args[i]);
+  ros::NodeHandle n;
+  g_node = &n;
+  g_output_topic = args[1];
+  // Put our API into the "mux" namespace, which the user should usually remap
+  ros::NodeHandle mux_nh("mux"), pnh("~");
+  pnh.getParam("lazy", g_lazy);
+
+  // Latched publisher for selected input topic name
+  g_pub_selected = mux_nh.advertise<std_msgs::String>(string("selected"), 1, true);
+
+  for (size_t i = 0; i < topics.size(); i++)
+  {
+    struct sub_info_t sub_info;
+    sub_info.msg = new ShapeShifter;
+    sub_info.topic_name = ros::names::resolve(topics[i]);
+    sub_info.sub = new ros::Subscriber(n.subscribe<ShapeShifter>(sub_info.topic_name, 10, boost::bind(in_cb, _1, sub_info.msg)));
+
+    g_subs.push_back(sub_info);
+  }
+  g_selected = g_subs.begin(); // select first topic to start
+  std_msgs::String t;
+  t.data = g_selected->topic_name;
+  g_pub_selected.publish(t);
+
+  // Backward compatibility
+  ros::ServiceServer ss = n.advertiseService(g_output_topic + string("_select"), sel_srv_cb_dep);
+  // New service
+  ros::ServiceServer ss_select = mux_nh.advertiseService(string("select"), sel_srv_cb);
+  ros::ServiceServer ss_add = mux_nh.advertiseService(string("add"), add_topic_cb);
+  ros::ServiceServer ss_list = mux_nh.advertiseService(string("list"), list_topic_cb);
+  ros::ServiceServer ss_del = mux_nh.advertiseService(string("delete"), del_topic_cb);
+  ros::spin();
+  for (list<struct sub_info_t>::iterator it = g_subs.begin();
+       it != g_subs.end();
+       ++it)
+  {
+    if (it->sub)
+      it->sub->shutdown();
+    delete it->sub;
+    delete it->msg;
+  }
+
+  g_subs.clear();
+  return 0;
+}
+

thirdparty/ros/ros_comm/tools/topic_tools/src/parse.cpp

@@ -0,0 +1,66 @@
+// Copyright (c) 2009, Willow Garage, Inc.
+// All rights reserved.
+// 
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// 
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of Willow Garage, Inc. nor the names of its
+//       contributors may be used to endorse or promote products derived from
+//       this software without specific prior written permission.
+// 
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+// Reusable parser routines
+
+#include "topic_tools/parse.h"
+
+#include <ros/console.h>
+
+namespace topic_tools
+{
+
+// Strip any leading namespace qualification from a topic (or other kind
+// of) ROS name
+bool 
+getBaseName(const std::string& full_name, std::string& base_name)
+{
+  std::string tmp = full_name;
+  int i = tmp.rfind('/');
+  // Strip off trailing slahes (are those legal anyway?)
+  while((tmp.size() > 0) && (i >= (int)(tmp.size() - 1)))
+  {
+    tmp = tmp.substr(0,tmp.size()-1);
+    i = tmp.rfind('/');
+  }
+
+  if(tmp.size() == 0)
+  {
+    ROS_ERROR("Base name extracted from \"%s\" is an empty string", 
+              full_name.c_str());
+    return false;
+  }
+
+  if(i < 0)
+    base_name = tmp;
+  else
+    base_name = tmp.substr(i+1, tmp.size()-i-1);
+
+  return true;
+}
+
+}

thirdparty/ros/ros_comm/tools/topic_tools/src/relay.cpp

@@ -0,0 +1,135 @@
+///////////////////////////////////////////////////////////////////////////////
+// relay just passes messages on. it can be useful if you're trying to ensure
+// that a message doesn't get sent twice over a wireless link, by having the 
+// relay catch the message and then do the fanout on the far side of the 
+// wireless link.
+//
+// Copyright (C) 2009, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Stanford University nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+/////////////////////////////////////////////////////////////////////////////
+
+
+#include <cstdio>
+#include "topic_tools/shape_shifter.h"
+#include "topic_tools/parse.h"
+
+using std::string;
+using std::vector;
+using namespace topic_tools;
+
+ros::NodeHandle *g_node = NULL;
+bool g_advertised = false;
+string g_input_topic;
+string g_output_topic;
+ros::Publisher g_pub;
+ros::Subscriber* g_sub;
+bool g_lazy;
+ros::TransportHints g_th;
+
+void conn_cb(const ros::SingleSubscriberPublisher&);
+void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event);
+
+void subscribe()
+{
+  g_sub = new ros::Subscriber(g_node->subscribe(g_input_topic, 10, &in_cb, g_th));
+}
+
+void conn_cb(const ros::SingleSubscriberPublisher&)
+{
+  // If we're in lazy subscribe mode, and the first subscriber just
+  // connected, then subscribe, #3389.
+  if(g_lazy && !g_sub)
+  {
+    ROS_DEBUG("lazy mode; resubscribing");
+    subscribe();
+  }
+}
+
+void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event)
+{
+  boost::shared_ptr<ShapeShifter const> const &msg = msg_event.getConstMessage();
+  boost::shared_ptr<const ros::M_string> const& connection_header = msg_event.getConnectionHeaderPtr();
+
+  if (!g_advertised)
+  {
+    // If the input topic is latched, make the output topic latched, #3385.
+    bool latch = false;
+    if (connection_header)
+    {
+      ros::M_string::const_iterator it = connection_header->find("latching");
+      if((it != connection_header->end()) && (it->second == "1"))
+      {
+        ROS_DEBUG("input topic is latched; latching output topic to match");
+        latch = true;
+      }
+    }
+    g_pub = msg->advertise(*g_node, g_output_topic, 10, latch, conn_cb);
+    g_advertised = true;
+    ROS_INFO("advertised as %s\n", g_output_topic.c_str());
+  }
+  // If we're in lazy subscribe mode, and nobody's listening, 
+  // then unsubscribe, #3389.
+  if(g_lazy && !g_pub.getNumSubscribers())
+  {
+    ROS_DEBUG("lazy mode; unsubscribing");
+    delete g_sub;
+    g_sub = NULL;
+  }
+  else
+    g_pub.publish(msg);
+}
+
+int main(int argc, char **argv)
+{
+  if (argc < 2)
+  {
+    printf("\nusage: relay IN_TOPIC [OUT_TOPIC]\n\n");
+    return 1;
+  }
+  std::string topic_name;
+  if(!getBaseName(string(argv[1]), topic_name))
+    return 1;
+  ros::init(argc, argv, topic_name + string("_relay"),
+            ros::init_options::AnonymousName);
+  if (argc == 2)
+    g_output_topic = string(argv[1]) + string("_relay");
+  else // argc == 3
+    g_output_topic = string(argv[2]);
+  g_input_topic = string(argv[1]);
+  ros::NodeHandle n;
+  g_node = &n;
+  
+  ros::NodeHandle pnh("~");
+  bool unreliable = false;
+  pnh.getParam("unreliable", unreliable);
+  pnh.getParam("lazy", g_lazy);
+  if (unreliable)
+    g_th.unreliable().reliable(); // Prefers unreliable, but will accept reliable.
+
+  subscribe();
+  ros::spin();
+  return 0;
+}
+

thirdparty/ros/ros_comm/tools/topic_tools/src/shape_shifter.cpp

@@ -0,0 +1,93 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+********************************************************************/
+
+#include <topic_tools/shape_shifter.h>
+
+using namespace topic_tools;
+
+bool ShapeShifter::uses_old_API_ = false;
+
+ShapeShifter::ShapeShifter()
+  :  typed(false),
+     msgBuf(NULL),
+     msgBufUsed(0),
+     msgBufAlloc(0)
+{ 
+}
+
+
+ShapeShifter::~ShapeShifter()
+{
+  if (msgBuf)
+    delete[] msgBuf;
+  
+  msgBuf = NULL;
+  msgBufAlloc = 0;
+}
+
+
+std::string const& ShapeShifter::getDataType()          const { return datatype; }
+
+
+std::string const& ShapeShifter::getMD5Sum()            const { return md5;   }
+
+
+std::string const& ShapeShifter::getMessageDefinition() const { return msg_def;  }
+
+
+void ShapeShifter::morph(const std::string& _md5sum, const std::string& _datatype, const std::string& _msg_def,
+                         const std::string& _latching)
+{
+  md5 = _md5sum;
+  datatype = _datatype;
+  msg_def = _msg_def;
+  latching = _latching;
+  typed = md5 != "*";
+}
+
+
+ros::Publisher ShapeShifter::advertise(ros::NodeHandle& nh, const std::string& topic, uint32_t queue_size_, bool latch, const ros::SubscriberStatusCallback &connect_cb) const
+{
+  ros::AdvertiseOptions opts(topic, queue_size_, getMD5Sum(), getDataType(), getMessageDefinition(), connect_cb);
+  opts.latch = latch;
+
+  return nh.advertise(opts);
+}
+
+
+uint32_t ShapeShifter::size() const
+{
+  return msgBufUsed;
+}
+

thirdparty/ros/ros_comm/tools/topic_tools/src/switch_mux.cpp

@@ -0,0 +1,73 @@
+///////////////////////////////////////////////////////////////////////////////
+// The mux package provides a generic multiplexer
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without 
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice, 
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright 
+//     notice, this list of conditions and the following disclaimer in the 
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Stanford University nor the names of its 
+//     contributors may be used to endorse or promote products derived from 
+//     this software without specific prior written permission.
+//   
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
+// POSSIBILITY OF SUCH DAMAGE.
+/////////////////////////////////////////////////////////////////////////////
+
+
+#include <cstdio>
+#include "ros/console.h"
+#include "ros/ros.h"
+#include "topic_tools/MuxSelect.h"
+#include "topic_tools/parse.h"
+using namespace std;
+using namespace ros;
+using namespace topic_tools;
+
+int main(int argc, char **argv)
+{
+  ROS_WARN("topic_tools/switch_mux is deprecated; please use topic_tools/mux_select instead");
+  if (argc < 3)
+  {
+    printf("usage: switch MUXED_TOPIC SELECT_TOPIC\n");
+    return 1;
+  }
+  std::string topic_name;
+  if(!getBaseName(string(argv[1]), topic_name))
+    return 1;
+  ros::init(argc, argv, topic_name + string("_switcher"));
+  ros::NodeHandle nh;
+  string srv_name = string(argv[1]) + "_select";
+  ROS_INFO("Waiting for service %s...\n", srv_name.c_str());
+  ros::service::waitForService(srv_name, -1);
+  ros::ServiceClient client = nh.serviceClient<MuxSelect>(srv_name);
+  MuxSelect cmd;
+  cmd.request.topic = argv[2];
+  if (client.call(cmd))
+  {
+    ROS_INFO("muxed topic %s successfully switched from %s to %s",
+             argv[1], cmd.response.prev_topic.c_str(),
+             cmd.request.topic.c_str());
+    return 0;
+  }
+  else
+  {
+    ROS_ERROR("failed to switch muxed topic %s to %s",
+              argv[1], cmd.request.topic.c_str());
+    return 1;
+  }
+}
+

thirdparty/ros/ros_comm/tools/topic_tools/src/throttle.cpp

@@ -0,0 +1,237 @@
+///////////////////////////////////////////////////////////////////////////////
+// throttle will transform a topic to have a limited number of bytes per second
+//
+// Copyright (C) 2009, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of Stanford University nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+/////////////////////////////////////////////////////////////////////////////
+
+
+// this could be made a lot smarter by trying to analyze and predict the
+// message stream density, etc., rather than just being greedy and stuffing
+// the output as fast as it can. 
+
+#include <cstdio>
+#include <cstdlib>
+#include <deque>
+#include "topic_tools/shape_shifter.h"
+#include "topic_tools/parse.h"
+
+using std::string;
+using std::vector;
+using std::deque;
+using namespace topic_tools;
+
+// TODO: move all these globals into a reasonable local scope
+ros::NodeHandle *g_node = NULL;
+uint32_t g_bps = 0; // bytes per second, not bits!
+ros::Duration g_period; // minimum inter-message period
+double g_window = 1.0; // 1 second window for starters
+bool g_advertised = false;
+string g_output_topic;
+string g_input_topic;
+ros::Publisher g_pub;
+ros::Subscriber* g_sub;
+bool g_use_messages;
+ros::Time g_last_time;
+bool g_use_wallclock;
+bool g_lazy;
+ros::TransportHints g_th;
+
+class Sent
+{
+public:
+  double t;
+  uint32_t len;
+  Sent(double _t, uint32_t _len) : t(_t), len(_len) { }
+};
+deque<Sent> g_sent;
+
+void conn_cb(const ros::SingleSubscriberPublisher&);
+void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event);
+
+void subscribe()
+{
+  g_sub = new ros::Subscriber(g_node->subscribe(g_input_topic, 10, &in_cb, g_th));
+}
+
+void conn_cb(const ros::SingleSubscriberPublisher&)
+{
+  // If we're in lazy subscribe mode, and the first subscriber just
+  // connected, then subscribe, #3546
+  if(g_lazy && !g_sub)
+  {
+    ROS_DEBUG("lazy mode; resubscribing");
+    subscribe();
+  }
+}
+
+void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event)
+{
+  boost::shared_ptr<ShapeShifter const> const &msg = msg_event.getConstMessage();
+  boost::shared_ptr<const ros::M_string> const& connection_header = msg_event.getConnectionHeaderPtr();
+
+  if (!g_advertised)
+  {
+    // If the input topic is latched, make the output topic latched
+    bool latch = false;
+    if (connection_header)
+    {
+      ros::M_string::const_iterator it = connection_header->find("latching");
+      if((it != connection_header->end()) && (it->second == "1"))
+      {
+        ROS_DEBUG("input topic is latched; latching output topic to match");
+        latch = true;
+      }
+    }
+    g_pub = msg->advertise(*g_node, g_output_topic, 10, latch, conn_cb);
+    g_advertised = true;
+    printf("advertised as %s\n", g_output_topic.c_str());
+  }
+  // If we're in lazy subscribe mode, and nobody's listening, 
+  // then unsubscribe, #3546.
+  if(g_lazy && !g_pub.getNumSubscribers())
+  {
+    ROS_DEBUG("lazy mode; unsubscribing");
+    delete g_sub;
+    g_sub = NULL;
+  }
+  else
+  {
+    if(g_use_messages)
+    {
+      ros::Time now;
+      if(g_use_wallclock)
+        now.fromSec(ros::WallTime::now().toSec());
+      else
+        now = ros::Time::now();
+      if((now - g_last_time) > g_period)
+      {
+        g_pub.publish(msg);
+        g_last_time = now;
+      }
+    }
+    else
+    {
+      // pop the front of the queue until it's within the window
+      ros::Time now;
+      if(g_use_wallclock)
+        now.fromSec(ros::WallTime::now().toSec());
+      else
+        now = ros::Time::now();
+      const double t = now.toSec();
+      while (!g_sent.empty() && g_sent.front().t < t - g_window)
+        g_sent.pop_front();
+      // sum up how many bytes are in the window
+      uint32_t bytes = 0;
+      for (deque<Sent>::iterator i = g_sent.begin(); i != g_sent.end(); ++i)
+        bytes += i->len;
+      if (bytes < g_bps)
+      {
+        g_pub.publish(msg);
+        g_sent.push_back(Sent(t, msg->size()));
+      }
+    }
+  }
+}
+
+#define USAGE "\nusage: \n"\
+           "  throttle messages IN_TOPIC MSGS_PER_SEC [OUT_TOPIC]]\n"\
+           "OR\n"\
+           "  throttle bytes IN_TOPIC BYTES_PER_SEC WINDOW [OUT_TOPIC]]\n\n"\
+           "  This program will drop messages from IN_TOPIC so that either: the \n"\
+           "  average bytes per second on OUT_TOPIC, averaged over WINDOW \n"\
+           "  seconds, remains below BYTES_PER_SEC, or: the minimum inter-message\n"\
+           "  period is 1/MSGS_PER_SEC. The messages are output \n"\
+           "  to OUT_TOPIC, or (if not supplied), to IN_TOPIC_throttle.\n\n"
+
+int main(int argc, char **argv)
+{
+  if(argc < 3)
+  {
+    puts(USAGE);
+    return 1;
+  }
+
+  g_input_topic = string(argv[2]);
+
+  std::string topic_name;
+  if(!getBaseName(string(argv[2]), topic_name))
+    return 1;
+
+  ros::init(argc, argv, topic_name + string("_throttle"),
+            ros::init_options::AnonymousName);
+  bool unreliable = false;
+  ros::NodeHandle pnh("~");
+  pnh.getParam("wall_clock", g_use_wallclock);
+  pnh.getParam("unreliable", unreliable);
+  pnh.getParam("lazy", g_lazy);
+
+  if (unreliable)
+    g_th.unreliable().reliable(); // Prefers unreliable, but will accept reliable.
+
+  if(!strcmp(argv[1], "messages"))
+    g_use_messages = true;
+  else if(!strcmp(argv[1], "bytes"))
+    g_use_messages = false;
+  else
+  {
+    puts(USAGE);
+    return 1;
+  }
+
+  if(g_use_messages && argc == 5)
+    g_output_topic = string(argv[4]);
+  else if(!g_use_messages && argc == 6)
+    g_output_topic = string(argv[5]);
+  else
+    g_output_topic = g_input_topic + "_throttle";
+
+  if(g_use_messages)
+  {
+    if(argc < 4)
+    {
+      puts(USAGE);
+      return 1;
+    }
+    g_period = ros::Duration(1.0/atof(argv[3]));
+  }
+  else
+  {
+    if(argc < 5)
+    {
+      puts(USAGE);
+      return 1;
+    }
+    g_bps = atoi(argv[3]);
+    g_window = atof(argv[4]);
+  }
+
+  ros::NodeHandle n;
+  g_node = &n;
+  subscribe();
+  ros::spin();
+  return 0;
+}
+