Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

image_transport/republish: add in_queue_size/out_queue_size parameters #149

Open
wants to merge 2 commits into
base: noetic-devel
Choose a base branch
from
Open
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 10 additions & 5 deletions image_transport/src/republish.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,22 +43,27 @@ int main(int argc, char** argv)
printf("Usage: %s in_transport in:=<in_base_topic> [out_transport] out:=<out_base_topic>\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<int>("out_queue_size", out_queue_size, 1);
int in_queue_size = 1;
pnh.param<int>("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();
}
Expand All @@ -71,13 +76,13 @@ int main(int argc, char** argv)
pluginlib::ClassLoader<Plugin> loader("image_transport", "image_transport::PublisherPlugin");
std::string lookup_name = Plugin::getLookupName(out_transport);
boost::shared_ptr<Plugin> 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();
}
Expand Down