00001 #include "image_transport/image_transport.h"
00002 #include "image_transport/publisher_plugin.h"
00003 #include <pluginlib/class_loader.h>
00004
00005 int main(int argc, char** argv)
00006 {
00007 ros::init(argc, argv, "image_republisher", ros::init_options::AnonymousName);
00008 if (argc < 2) {
00009 printf("Usage: %s in_transport in:=<in_base_topic> [out_transport] out:=<out_base_topic>\n", argv[0]);
00010 return 0;
00011 }
00012 ros::NodeHandle nh;
00013 std::string in_topic = nh.resolveName("in");
00014 std::string in_transport = argv[1];
00015 std::string out_topic = nh.resolveName("out");
00016
00017 image_transport::ImageTransport it(nh);
00018 image_transport::Subscriber sub;
00019
00020 if (argc < 3) {
00021
00022 image_transport::Publisher pub = it.advertise(out_topic, 1);
00023
00024
00025 typedef void (image_transport::Publisher::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const;
00026 PublishMemFn pub_mem_fn = &image_transport::Publisher::publish;
00027 sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, _1), ros::VoidPtr(), in_transport);
00028
00029 ros::spin();
00030 }
00031 else {
00032
00033 std::string out_transport = argv[2];
00034
00035
00036 typedef image_transport::PublisherPlugin Plugin;
00037 pluginlib::ClassLoader<Plugin> loader("image_transport", "image_transport::PublisherPlugin");
00038 std::string lookup_name = Plugin::getLookupName(out_transport);
00039 boost::shared_ptr<Plugin> pub( loader.createClassInstance(lookup_name) );
00040 pub->advertise(nh, out_topic, 1, image_transport::SubscriberStatusCallback(),
00041 image_transport::SubscriberStatusCallback(), ros::VoidPtr(), false);
00042
00043
00044 typedef void (Plugin::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const;
00045 PublishMemFn pub_mem_fn = &Plugin::publish;
00046 sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), _1), pub, in_transport);
00047
00048 ros::spin();
00049 }
00050
00051 return 0;
00052 }