image_transport: add 'republish'nodelet

ros-perception · Feb 1, 2018 · 0649ec6 · 0649ec6
1 parent 5a3098b
commit 0649ec6
Show file tree

Hide file tree

Showing 3 changed files with 177 additions and 72 deletions.
diff --git 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
@@ -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 <[email protected]>
+ */
+
+
+#include <nodelet/loader.h>
+#include <ros/ros.h>
 
-#include "image_transport/image_transport.h"
-#include "image_transport/publisher_plugin.h"
-#include <pluginlib/class_loader.h>
 
 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:=<in_base_topic> [out_transport] out:=<out_base_topic>\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<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(),
-                   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
@@ -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 <[email protected]>
+ */
+
+
+#include <image_transport/image_transport.h>
+#include <image_transport/nodelet_lazy.h>
+#include <image_transport/publisher_plugin.h>
+#include <pluginlib/class_loader.h>
+
+
+namespace image_transport
+{
+class RepublishNodelet : public NodeletLazy
+{
+protected:
+  boost::shared_ptr<ImageTransport> it_;
+  Publisher pub_;
+  boost::shared_ptr<pluginlib::ClassLoader<PublisherPlugin> > loader_;
+  boost::shared_ptr<PublisherPlugin> pub_plugin_;
+  Subscriber sub_;
+  std::string in_transport_, out_transport_;
+
+  virtual void onInit()
+  {
+    NodeletLazy::onInit();
+
+    pnh_->getParam("in_transport", in_transport_);
+    pnh_->param<std::string>("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<PublisherPlugin>(
+          "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<PublisherPlugin>(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/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(image_transport::RepublishNodelet, nodelet::Nodelet)