From 5a3098b0bab1967e5aeba811a39c9b378c8a13b1 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Fri, 2 Feb 2018 02:53:26 +0900 Subject: [PATCH 1/4] image_transport: add nodelet_lazy for image_transport --- image_transport/CMakeLists.txt | 4 +- .../include/image_transport/nodelet_lazy.h | 348 ++++++++++++++++++ image_transport/nodelet_plugins.xml | 9 + image_transport/package.xml | 7 +- 4 files changed, 366 insertions(+), 2 deletions(-) create mode 100644 image_transport/include/image_transport/nodelet_lazy.h create mode 100644 image_transport/nodelet_plugins.xml diff --git a/image_transport/CMakeLists.txt b/image_transport/CMakeLists.txt index 6e43f2fd..53e9980c 100644 --- a/image_transport/CMakeLists.txt +++ b/image_transport/CMakeLists.txt @@ -4,6 +4,8 @@ project(image_transport) find_package(catkin REQUIRED COMPONENTS message_filters + nodelet + nodelet_topic_tools pluginlib rosconsole roscpp @@ -64,6 +66,6 @@ install(TARGETS list_transports republish ) # add xml file -install(FILES default_plugins.xml +install(FILES default_plugins.xml nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/image_transport/include/image_transport/nodelet_lazy.h b/image_transport/include/image_transport/nodelet_lazy.h new file mode 100644 index 00000000..96e141f7 --- /dev/null +++ b/image_transport/include/image_transport/nodelet_lazy.h @@ -0,0 +1,348 @@ +// -*- mode: C++ -*- +/********************************************************************* + * 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: Ryohei Ueda + * Yuki Furuta + */ + + +#ifndef IMAGE_TRANSPORT_NODELET_LAZY_H__ +#define IMAGE_TRANSPORT_NODELET_LAZY_H__ + + +#include +#include +#include +#include + +/** @brief + * Nodelet to automatically subscribe/unsubscribe + * image/camera topics according to subscription of advertised topics. + * + * It's important not to subscribe topic if no output is required. + * + * In order to watch advertised topics, need to use advertise template method. + * And create subscribers in subscribe() and shutdown them in unsubscribed(). + * + */ +namespace image_transport +{ +class NodeletLazy : public nodelet_topic_tools::NodeletLazy +{ +protected: + + /** @brief + * callback function which is called when a new subscriber comes + */ + virtual void imageConnectionCallback( + const SingleSubscriberPublisher&) + { + if (verbose_connection_) + { + NODELET_INFO("New image connection or disconnection is detected"); + } + if (lazy_) + { + boost::mutex::scoped_lock lock(connection_mutex_); + for (size_t i = 0; i < image_publishers_.size(); i++) + { + Publisher pub = image_publishers_[i]; + if (pub.getNumSubscribers() > 0) + { + if (connection_status_ != nodelet_topic_tools::SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Subscribe input topics"); + } + subscribe(); + connection_status_ = nodelet_topic_tools::SUBSCRIBED; + } + if (!ever_subscribed_) + { + ever_subscribed_ = true; + } + return; + } + } + if (connection_status_ == nodelet_topic_tools::SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Unsubscribe input topics"); + } + unsubscribe(); + connection_status_ = nodelet_topic_tools::NOT_SUBSCRIBED; + } + } + } + + /** @brief + * callback function which is called when a new subscriber comes + */ + virtual void imagePluginConnectionCallback( + const SingleSubscriberPublisher&) + { + if (verbose_connection_) + { + NODELET_INFO("New image connection or disconnection is detected"); + } + if (lazy_) + { + boost::mutex::scoped_lock lock(connection_mutex_); + for (size_t i = 0; i < image_publisher_plugins_.size(); i++) + { + boost::weak_ptr + weak_pub = image_publisher_plugins_[i]; + if (boost::shared_ptr pub = weak_pub.lock()) + { + if (pub->getNumSubscribers() > 0) + { + if (connection_status_ != nodelet_topic_tools::SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Subscribe input topics"); + } + subscribe(); + connection_status_ = nodelet_topic_tools::SUBSCRIBED; + } + if (!ever_subscribed_) + { + ever_subscribed_ = true; + } + return; + } + } + else + { + NODELET_ERROR("Image Plugin is already deallocated"); + } + } + if (connection_status_ == nodelet_topic_tools::SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Unsubscribe input topics"); + } + unsubscribe(); + connection_status_ = nodelet_topic_tools::NOT_SUBSCRIBED; + } + } + } + + void cameraConnectionCallbackImpl() + { + if (verbose_connection_) + { + NODELET_INFO("New camera connection or disconnection is detected"); + } + if (lazy_) + { + boost::mutex::scoped_lock lock(connection_mutex_); + for (size_t i = 0; i < camera_publishers_.size(); i++) + { + CameraPublisher pub = camera_publishers_[i]; + if (pub.getNumSubscribers() > 0) + { + if (connection_status_ != nodelet_topic_tools::SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Subscribe input topics"); + } + subscribe(); + connection_status_ = nodelet_topic_tools::SUBSCRIBED; + } + if (!ever_subscribed_) + { + ever_subscribed_ = true; + } + return; + } + } + if (connection_status_ == nodelet_topic_tools::SUBSCRIBED) + { + if (verbose_connection_) + { + NODELET_INFO("Unsubscribe input topics"); + } + unsubscribe(); + connection_status_ = nodelet_topic_tools::NOT_SUBSCRIBED; + } + } + } + + /** @brief + * callback function which is called when a new subscriber comes + */ + virtual void cameraConnectionCallback( + const SingleSubscriberPublisher&) + { + cameraConnectionCallbackImpl(); + } + + /** @brief + * callback function which is called when a new subscriber comes + */ + virtual void cameraInfoConnectionCallback( + const ros::SingleSubscriberPublisher&) + { + cameraConnectionCallbackImpl(); + } + + /** @brief + * Update the list of Publishers created by this method. + * It automatically reads latch boolean parameter from nh and + * publish topic with appropriate latch parameter. + * + * @param nh NodeHandle. + * @param topic topic name to advertise. + * @param queue_size queue size for publisher. + * @param latch set true if latch topic publication. + * @return Publisher for the advertised topic. + */ + Publisher + advertiseImage(ros::NodeHandle& nh, + const std::string& topic, + int queue_size, + bool latch=false) + { + boost::mutex::scoped_lock lock(connection_mutex_); + SubscriberStatusCallback connect_cb + = boost::bind(&NodeletLazy::imageConnectionCallback, this, _1); + SubscriberStatusCallback disconnect_cb + = boost::bind(&NodeletLazy::imageConnectionCallback, this, _1); + + Publisher pub = ImageTransport(nh).advertise( + topic, queue_size, + connect_cb, disconnect_cb, + ros::VoidPtr(), + latch); + image_publishers_.push_back(pub); + return pub; + } + + /** @brief + * Update the list of Publishers created by this method. + * It automatically reads latch boolean parameter from nh and + * publish topic with appropriate latch parameter. + * + * @param nh NodeHandle. + * @param topic topic name to advertise. + * @param queue_size queue size for publisher. + * @param latch set true if latch topic publication. + * @param transport transport name for advertised topic + */ + boost::shared_ptr + advertiseImage(ros::NodeHandle& nh, + const std::string& topic, + int queue_size, + boost::weak_ptr plugin, + bool latch=false) + { + boost::mutex::scoped_lock lock(connection_mutex_); + SubscriberStatusCallback connect_cb + = boost::bind(&NodeletLazy::imagePluginConnectionCallback, this, _1); + SubscriberStatusCallback disconnect_cb + = boost::bind(&NodeletLazy::imagePluginConnectionCallback, this, _1); + std::string resolved_topic = nh.resolveName(topic); + if (boost::shared_ptr shared_plugin = plugin.lock()) + { + shared_plugin->advertise( + nh, resolved_topic, queue_size, + connect_cb, disconnect_cb, + ros::VoidPtr(), + latch); + image_publisher_plugins_.push_back(plugin); + } + } + + /** @brief + * Update the list of Publishers created by this method. + * It automatically reads latch boolean parameter from nh and + * publish topic with appropriate latch parameter. + * + * @param nh NodeHandle. + * @param topic topic name to advertise. + * @param queue_size queue size for publisher. + * @param latch set true if latch topic publication. + * @return CameraPublisher for the advertised topic. + */ + CameraPublisher + advertiseCamera(ros::NodeHandle& nh, + const std::string& topic, + int queue_size, + bool latch=false) + { + boost::mutex::scoped_lock lock(connection_mutex_); + SubscriberStatusCallback connect_cb + = boost::bind(&NodeletLazy::cameraConnectionCallback, this, _1); + SubscriberStatusCallback disconnect_cb + = boost::bind(&NodeletLazy::cameraConnectionCallback, this, _1); + ros::SubscriberStatusCallback info_connect_cb + = boost::bind(&NodeletLazy::cameraInfoConnectionCallback, this, _1); + ros::SubscriberStatusCallback info_disconnect_cb + = boost::bind(&NodeletLazy::cameraInfoConnectionCallback, this, _1); + + CameraPublisher pub = ImageTransport(nh).advertiseCamera( + topic, queue_size, + connect_cb, disconnect_cb, + info_connect_cb, info_disconnect_cb, + ros::VoidPtr(), + latch); + camera_publishers_.push_back(pub); + return pub; + } + + /** @brief + * List of monitoring image publishers + */ + std::vector image_publishers_; + + /** @brief + * List of monitoring image publishers + */ + std::vector > image_publisher_plugins_; + + /** @brief + * List of monitoring camera publishers + */ + std::vector camera_publishers_; +}; + +} // namespace image_transport + +#endif // IMAGE_TRANSPORT_NODELET_LAZY_H__ diff --git a/image_transport/nodelet_plugins.xml b/image_transport/nodelet_plugins.xml new file mode 100644 index 00000000..a6ec069c --- /dev/null +++ b/image_transport/nodelet_plugins.xml @@ -0,0 +1,9 @@ + + + + Nodelet to republish image using specified transport + + + diff --git a/image_transport/package.xml b/image_transport/package.xml index e77d7e40..90a4480c 100644 --- a/image_transport/package.xml +++ b/image_transport/package.xml @@ -19,10 +19,13 @@ + catkin - + + nodelet + nodelet_topic_tools message_filters pluginlib rosconsole @@ -30,6 +33,8 @@ roslib sensor_msgs + nodelet + nodelet_topic_tools message_filters pluginlib rosconsole From 0649ec629942ff154c09c5b29f3c9bf97c4c31f4 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Fri, 2 Feb 2018 02:54:12 +0900 Subject: [PATCH 2/4] 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) From fd9bf19068f311e460e575bff8fa550b231aa8f4 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Mon, 24 Dec 2018 17:42:16 +0900 Subject: [PATCH 3/4] fix republish nodelet --- image_transport/src/republish.cpp | 46 ++++++++++++++++++++++- image_transport/src/republish_nodelet.cpp | 16 +++++--- 2 files changed, 55 insertions(+), 7 deletions(-) diff --git a/image_transport/src/republish.cpp b/image_transport/src/republish.cpp index 82eb8280..e7533039 100644 --- a/image_transport/src/republish.cpp +++ b/image_transport/src/republish.cpp @@ -37,10 +37,50 @@ */ +#include +#include #include #include +namespace ns { + std::string validate(const std::string& name) + { + std::vector list_all, list_nonempty; + std::string delim("/"); + boost::split(list_all, name, boost::is_any_of(delim)); + for (std::vector::iterator it = list_all.begin(); it != list_all.end(); ++it) + { + if (!it->empty()) list_nonempty.push_back(*it); + } + return delim + boost::algorithm::join(list_nonempty, delim); + } + + std::string join(const std::string& parent, const std::string& child) + { + return validate(parent + "/" + child); + } + + std::string basename(const std::string& name) + { + std::vector list; + boost::split(list, name,boost::is_any_of("/")); + if (list.size() > 0) return list[list.size() - 1]; + else return name; + } +} + + +void remapToPrivate(const nodelet::M_string& src, nodelet::M_string& dst) +{ + for (nodelet::M_string::const_iterator it = src.begin(); it != src.end(); ++it) + { + std::string key = ns::join(ros::this_node::getName(), ns::basename(it->first)); + dst[key] = it->second; + } +} + + int main(int argc, char** argv) { ros::init(argc, argv, "image_republisher", ros::init_options::AnonymousName); @@ -51,6 +91,7 @@ int main(int argc, char** argv) return 1; } + // set transport ros::param::set("~in_transport", argv[1]); if (argc >= 3) { @@ -58,9 +99,12 @@ int main(int argc, char** argv) } nodelet::Loader loader; - nodelet::M_string remappings(ros::names::getRemappings()); nodelet::V_string nargv; + // remap 'in', 'out' topic into ~in, ~out for backward compatibility + nodelet::M_string remappings; + remapToPrivate(ros::names::getRemappings(), remappings); + loader.load(ros::this_node::getName(), "image_transport/Republish", remappings, nargv); diff --git a/image_transport/src/republish_nodelet.cpp b/image_transport/src/republish_nodelet.cpp index 0988802e..97808361 100644 --- a/image_transport/src/republish_nodelet.cpp +++ b/image_transport/src/republish_nodelet.cpp @@ -59,14 +59,18 @@ class RepublishNodelet : public NodeletLazy { NodeletLazy::onInit(); - pnh_->getParam("in_transport", in_transport_); - pnh_->param("out_transport", out_transport_, std::string()); + if (!pnh_->getParam("in_transport", in_transport_)) + { + NODELET_FATAL("you must set '~in_transport' parameter."); + return; + } + pnh_->param("out_transport", out_transport_, std::string()); - it_.reset(new ImageTransport(*nh_)); + it_.reset(new ImageTransport(*pnh_)); if (out_transport_.empty()) { - pub_ = advertiseImage(*nh_, "out", 1); + pub_ = advertiseImage(*pnh_, "out", 1); } else { @@ -75,7 +79,7 @@ class RepublishNodelet : public NodeletLazy std::string lookup_name = PublisherPlugin::getLookupName(out_transport_); pub_plugin_ = loader_->createInstance(lookup_name); - advertiseImage(*nh_, "out", 1, boost::weak_ptr(pub_plugin_)); + advertiseImage(*pnh_, "out", 1, boost::weak_ptr(pub_plugin_)); } onInitPostProcess(); @@ -83,7 +87,7 @@ class RepublishNodelet : public NodeletLazy virtual void subscribe() { - std::string in_topic = nh_->resolveName("in"); + std::string in_topic = pnh_->resolveName("in"); if (out_transport_.empty()) { // Use Publisher::publish as the subscriber callback From f62eb62f0237808dd546f3c5f467d18b500745d3 Mon Sep 17 00:00:00 2001 From: Furushchev Date: Mon, 24 Dec 2018 17:51:03 +0900 Subject: [PATCH 4/4] republish: warn no remap --- image_transport/src/republish_nodelet.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/image_transport/src/republish_nodelet.cpp b/image_transport/src/republish_nodelet.cpp index 97808361..0af69d60 100644 --- a/image_transport/src/republish_nodelet.cpp +++ b/image_transport/src/republish_nodelet.cpp @@ -88,6 +88,12 @@ class RepublishNodelet : public NodeletLazy virtual void subscribe() { std::string in_topic = pnh_->resolveName("in"); + + if (in_topic == pnh_->getNamespace() + "/in") + { + NODELET_WARN_ONCE("Input topic '~in' is not remapped."); + } + if (out_transport_.empty()) { // Use Publisher::publish as the subscriber callback