From 47b96b0fc498853cc30dd35756e715a2b95d1ce3 Mon Sep 17 00:00:00 2001 From: Yuki Furuta Date: Thu, 26 Dec 2019 22:51:01 +0900 Subject: [PATCH] image_transport/republish: add in_queue_size/out_queue_size parameters --- image_transport/src/republish_nodelet.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/image_transport/src/republish_nodelet.cpp b/image_transport/src/republish_nodelet.cpp index 0af69d60..24e20a0a 100644 --- a/image_transport/src/republish_nodelet.cpp +++ b/image_transport/src/republish_nodelet.cpp @@ -54,11 +54,15 @@ class RepublishNodelet : public NodeletLazy boost::shared_ptr pub_plugin_; Subscriber sub_; std::string in_transport_, out_transport_; + int in_queue_size_, out_queue_size_; virtual void onInit() { NodeletLazy::onInit(); + pnh_->param("in_queue_size", in_queue_size_, 1); + pnh_->param("out_queue_size", out_queue_size_, 1); + if (!pnh_->getParam("in_transport", in_transport_)) { NODELET_FATAL("you must set '~in_transport' parameter."); @@ -70,7 +74,7 @@ class RepublishNodelet : public NodeletLazy if (out_transport_.empty()) { - pub_ = advertiseImage(*pnh_, "out", 1); + pub_ = advertiseImage(*pnh_, "out", out_queue_size_); } else { @@ -79,7 +83,7 @@ class RepublishNodelet : public NodeletLazy std::string lookup_name = PublisherPlugin::getLookupName(out_transport_); pub_plugin_ = loader_->createInstance(lookup_name); - advertiseImage(*pnh_, "out", 1, boost::weak_ptr(pub_plugin_)); + advertiseImage(*pnh_, "out", out_queue_size_, boost::weak_ptr(pub_plugin_)); } onInitPostProcess(); @@ -100,7 +104,7 @@ class RepublishNodelet : public NodeletLazy typedef void (Publisher::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; PublishMemFn pub_mem_fn = &Publisher::publish; sub_ = it_->subscribe( - in_topic, 1, + in_topic, in_queue_size_, boost::bind(pub_mem_fn, &pub_, _1), ros::VoidPtr(), in_transport_); } @@ -109,7 +113,7 @@ class RepublishNodelet : public NodeletLazy typedef void (PublisherPlugin::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; PublishMemFn pub_mem_fn = &PublisherPlugin::publish; sub_ = it_->subscribe( - in_topic, 1, + in_topic, in_queue_size_, boost::bind(pub_mem_fn, pub_plugin_.get(), _1), pub_plugin_, in_transport_); }