Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "image_transport/image_transport.h"
00036 #include "image_transport/publisher_plugin.h"
00037 #include <pluginlib/class_loader.h>
00038
00039 int main(int argc, char** argv)
00040 {
00041 ros::init(argc, argv, "image_republisher", ros::init_options::AnonymousName);
00042 if (argc < 2) {
00043 printf("Usage: %s in_transport in:=<in_base_topic> [out_transport] out:=<out_base_topic>\n", argv[0]);
00044 return 0;
00045 }
00046 ros::NodeHandle nh;
00047 std::string in_topic = nh.resolveName("in");
00048 std::string in_transport = argv[1];
00049 std::string out_topic = nh.resolveName("out");
00050
00051 image_transport::ImageTransport it(nh);
00052 image_transport::Subscriber sub;
00053
00054 if (argc < 3) {
00055
00056 image_transport::Publisher pub = it.advertise(out_topic, 1);
00057
00058
00059 typedef void (image_transport::Publisher::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const;
00060 PublishMemFn pub_mem_fn = &image_transport::Publisher::publish;
00061 sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, &pub, _1), ros::VoidPtr(), in_transport);
00062
00063 ros::spin();
00064 }
00065 else {
00066
00067 std::string out_transport = argv[2];
00068
00069
00070 typedef image_transport::PublisherPlugin Plugin;
00071 pluginlib::ClassLoader<Plugin> loader("image_transport", "image_transport::PublisherPlugin");
00072 std::string lookup_name = Plugin::getLookupName(out_transport);
00073 boost::shared_ptr<Plugin> pub( loader.createInstance(lookup_name) );
00074 pub->advertise(nh, out_topic, 1, image_transport::SubscriberStatusCallback(),
00075 image_transport::SubscriberStatusCallback(), ros::VoidPtr(), false);
00076
00077
00078 typedef void (Plugin::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const;
00079 PublishMemFn pub_mem_fn = &Plugin::publish;
00080 sub = it.subscribe(in_topic, 1, boost::bind(pub_mem_fn, pub.get(), _1), pub, in_transport);
00081
00082 ros::spin();
00083 }
00084
00085 return 0;
00086 }