diff --git a/image_transport/src/republish.cpp b/image_transport/src/republish.cpp index 90946743..2c9573fb 100644 --- a/image_transport/src/republish.cpp +++ b/image_transport/src/republish.cpp @@ -43,22 +43,27 @@ int main(int argc, char** argv) printf("Usage: %s in_transport in:= [out_transport] out:=\n", argv[0]); return 0; } - ros::NodeHandle nh; + ros::NodeHandle nh(""), pnh("~"); std::string in_topic = nh.resolveName("in"); std::string in_transport = argv[1]; std::string out_topic = nh.resolveName("out"); + int out_queue_size = 1; + pnh.param("out_queue_size", out_queue_size, 1); + int in_queue_size = 1; + pnh.param("in_queue_size", in_queue_size, 1); + image_transport::ImageTransport it(nh); image_transport::Subscriber sub; if (argc < 3) { // Use all available transports for output - image_transport::Publisher pub = it.advertise(out_topic, 1); + image_transport::Publisher pub = it.advertise(out_topic, out_queue_size); // Use Publisher::publish as the subscriber callback typedef void (image_transport::Publisher::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; PublishMemFn pub_mem_fn = &image_transport::Publisher::publish; - sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, boost::placeholders::_1), ros::VoidPtr(), in_transport); + sub = it.subscribe(in_topic, in_queue_size, boost::bind(pub_mem_fn, &pub, boost::placeholders::_1), ros::VoidPtr(), in_transport); ros::spin(); } @@ -71,13 +76,13 @@ int main(int argc, char** argv) pluginlib::ClassLoader loader("image_transport", "image_transport::PublisherPlugin"); std::string lookup_name = Plugin::getLookupName(out_transport); boost::shared_ptr pub( loader.createInstance(lookup_name) ); - pub->advertise(nh, out_topic, 1, image_transport::SubscriberStatusCallback(), + pub->advertise(nh, out_topic, out_queue_size, image_transport::SubscriberStatusCallback(), image_transport::SubscriberStatusCallback(), ros::VoidPtr(), false); // Use PublisherPlugin::publish as the subscriber callback typedef void (Plugin::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; PublishMemFn pub_mem_fn = &Plugin::publish; - sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), boost::placeholders::_1), pub, in_transport); + sub = it.subscribe(in_topic, in_queue_size, boost::bind(pub_mem_fn, pub.get(), boost::placeholders::_1), pub, in_transport); ros::spin(); }