From 0649ec629942ff154c09c5b29f3c9bf97c4c31f4 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Fri, 2 Feb 2018 02:54:12 +0900 Subject: [PATCH] image_transport: add 'republish'nodelet --- image_transport/CMakeLists.txt | 8 +- image_transport/src/republish.cpp | 125 ++++++++++------------ image_transport/src/republish_nodelet.cpp | 116 ++++++++++++++++++++ 3 files changed, 177 insertions(+), 72 deletions(-) create mode 100644 image_transport/src/republish_nodelet.cpp diff --git a/image_transport/CMakeLists.txt b/image_transport/CMakeLists.txt index 53e9980c..7f311528 100644 --- a/image_transport/CMakeLists.txt +++ b/image_transport/CMakeLists.txt @@ -43,7 +43,11 @@ target_link_libraries(${PROJECT_NAME} add_library(${PROJECT_NAME}_plugins src/manifest.cpp src/raw_publisher.cpp) target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME}) -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins +# republish nodelet +add_library(${PROJECT_NAME}_nodelet src/republish_nodelet.cpp) +target_link_libraries(${PROJECT_NAME}_nodelet ${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins ${PROJECT_NAME}_nodelet DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} COMPONENT main ) @@ -53,7 +57,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ # add two execs add_executable(republish src/republish.cpp) -target_link_libraries(republish ${PROJECT_NAME}) +target_link_libraries(republish ${PROJECT_NAME} ${PROJECT_NAME}_nodelet) add_executable(list_transports src/list_transports.cpp) target_link_libraries(list_transports diff --git a/image_transport/src/republish.cpp b/image_transport/src/republish.cpp index 14fb2761..82eb8280 100644 --- a/image_transport/src/republish.cpp +++ b/image_transport/src/republish.cpp @@ -1,86 +1,71 @@ +// -*- mode: C++ -*- /********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2009, Willow Garage, Inc. -* All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* * Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* * Redistributions in binary form must reproduce the above -* copyright notice, this list of conditions and the following -* disclaimer in the documentation and/or other materials provided -* with the distribution. -* * Neither the name of the Willow Garage nor the names of its -* contributors may be used to endorse or promote products derived -* from this software without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -*********************************************************************/ + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + * Author: Yuki Furuta + */ + + +#include +#include -#include "image_transport/image_transport.h" -#include "image_transport/publisher_plugin.h" -#include int main(int argc, char** argv) { ros::init(argc, argv, "image_republisher", ros::init_options::AnonymousName); - if (argc < 2) { + + if (argc < 2) + { printf("Usage: %s in_transport in:= [out_transport] out:=\n", argv[0]); - return 0; + return 1; } - ros::NodeHandle nh; - std::string in_topic = nh.resolveName("in"); - std::string in_transport = argv[1]; - std::string out_topic = nh.resolveName("out"); - 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); - - // 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, _1), ros::VoidPtr(), in_transport); - - ros::spin(); + ros::param::set("~in_transport", argv[1]); + if (argc >= 3) + { + ros::param::set("~out_transport", argv[2]); } - else { - // Use one specific transport for output - std::string out_transport = argv[2]; - // Load transport plugin - typedef image_transport::PublisherPlugin Plugin; - 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(), - image_transport::SubscriberStatusCallback(), ros::VoidPtr(), false); + nodelet::Loader loader; + nodelet::M_string remappings(ros::names::getRemappings()); + nodelet::V_string nargv; - // 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(), _1), pub, in_transport); + loader.load(ros::this_node::getName(), + "image_transport/Republish", + remappings, nargv); - ros::spin(); - } + ros::spin(); return 0; } diff --git a/image_transport/src/republish_nodelet.cpp b/image_transport/src/republish_nodelet.cpp new file mode 100644 index 00000000..0988802e --- /dev/null +++ b/image_transport/src/republish_nodelet.cpp @@ -0,0 +1,116 @@ +// -*- mode: C++ -*- +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, JSK Lab + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/o2r other materials provided + * with the distribution. + * * Neither the name of the JSK Lab nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ +/* + * Author: Yuki Furuta + */ + + +#include +#include +#include +#include + + +namespace image_transport +{ +class RepublishNodelet : public NodeletLazy +{ +protected: + boost::shared_ptr it_; + Publisher pub_; + boost::shared_ptr > loader_; + boost::shared_ptr pub_plugin_; + Subscriber sub_; + std::string in_transport_, out_transport_; + + virtual void onInit() + { + NodeletLazy::onInit(); + + pnh_->getParam("in_transport", in_transport_); + pnh_->param("out_transport", out_transport_, std::string()); + + it_.reset(new ImageTransport(*nh_)); + + if (out_transport_.empty()) + { + pub_ = advertiseImage(*nh_, "out", 1); + } + else + { + loader_.reset(new pluginlib::ClassLoader( + "image_transport", "image_transport::PublisherPlugin")); + std::string lookup_name = PublisherPlugin::getLookupName(out_transport_); + pub_plugin_ = loader_->createInstance(lookup_name); + + advertiseImage(*nh_, "out", 1, boost::weak_ptr(pub_plugin_)); + } + + onInitPostProcess(); + } + + virtual void subscribe() + { + std::string in_topic = nh_->resolveName("in"); + if (out_transport_.empty()) + { + // Use Publisher::publish as the subscriber callback + typedef void (Publisher::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; + PublishMemFn pub_mem_fn = &Publisher::publish; + sub_ = it_->subscribe( + in_topic, 1, + boost::bind(pub_mem_fn, &pub_, _1), + ros::VoidPtr(), in_transport_); + } + else + { + typedef void (PublisherPlugin::*PublishMemFn)(const sensor_msgs::ImageConstPtr&) const; + PublishMemFn pub_mem_fn = &PublisherPlugin::publish; + sub_ = it_->subscribe( + in_topic, 1, + boost::bind(pub_mem_fn, pub_plugin_.get(), _1), + pub_plugin_, in_transport_); + } + } + + virtual void unsubscribe() + { + sub_.shutdown(); + } +}; +} + +#include +PLUGINLIB_EXPORT_CLASS(image_transport::RepublishNodelet, nodelet::Nodelet)