Added rclcpp component to Republish (ros-perception#275)

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

image_transport/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(sensor_msgs REQUIRED)
 
 # Build image_transport library
@@ -57,11 +58,24 @@ target_link_libraries(list_transports
   ${PROJECT_NAME}
   pluginlib::pluginlib)
 
-# Build republish
-add_executable(republish src/republish.cpp)
-target_link_libraries(republish
+# # Build republish
+add_library(republish_node SHARED
+  src/republish.cpp
+)
+target_link_libraries(republish_node
   ${PROJECT_NAME}
-  pluginlib::pluginlib)
+)
+ament_target_dependencies(republish_node
+  pluginlib
+  rclcpp_components
+  rclcpp
+)
+rclcpp_components_register_node(republish_node
+  PLUGIN "image_transport::Republisher"
+  EXECUTABLE republish
+)
+
+target_compile_definitions(republish_node PRIVATE "IMAGE_TRANSPORT_BUILDING_DLL")
 
 # Install plugin descriptions
 pluginlib_export_plugin_description_file(image_transport default_plugins.xml)
@@ -77,10 +91,16 @@ install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME})
 
 # Install executables
 install(
-  TARGETS list_transports republish
+  TARGETS list_transports
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
+install(TARGETS
+  republish_node
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
 # Install include directories
 install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 

image_transport/include/image_transport/republish.hpp

@@ -0,0 +1,62 @@
+// Copyright (c) 2023 Open Source Robotics Foundation, Inc.
+//
+// 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 HOLDER 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.
+
+#ifndef IMAGE_TRANSPORT__REPUBLISH_HPP_
+#define IMAGE_TRANSPORT__REPUBLISH_HPP_
+
+#include <memory>
+
+#include "image_transport/image_transport.hpp"
+#include "image_transport/visibility_control.hpp"
+
+#include <pluginlib/class_loader.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+namespace image_transport
+{
+class Republisher : public rclcpp::Node
+{
+public:
+  /// Constructor
+  IMAGE_TRANSPORT_PUBLIC
+  explicit Republisher(const rclcpp::NodeOptions & options);
+
+private:
+  void initialize();
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  bool initialized_{false};
+  image_transport::Subscriber sub;
+  image_transport::Publisher pub;
+  pluginlib::UniquePtr<image_transport::PublisherPlugin> instance;
+  std::shared_ptr<pluginlib::ClassLoader<image_transport::PublisherPlugin>> loader;
+};
+
+}  // namespace image_transport
+#endif  // IMAGE_TRANSPORT__REPUBLISH_HPP_

image_transport/package.xml

@@ -26,6 +26,7 @@
   <depend>message_filters</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>

image_transport/src/republish.cpp

@@ -26,53 +26,90 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#include <memory>
+#include "image_transport/republish.hpp"
+
+#include <chrono>
 #include <string>
-#include <utility>
 
-#include "pluginlib/class_loader.hpp"
+#include <pluginlib/class_loader.hpp>
 
-#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/rclcpp.hpp>
 
 #include "image_transport/image_transport.hpp"
 #include "image_transport/publisher_plugin.hpp"
 
-int main(int argc, char ** argv)
-{
-  auto vargv = rclcpp::init_and_remove_ros_arguments(argc, argv);
+using namespace std::chrono_literals;
 
-  if (vargv.size() < 2) {
-    printf(
-      "Usage: %s in_transport in:=<in_base_topic> [out_transport] out:=<out_base_topic>\n",
-      argv[0]);
-    return 0;
-  }
+namespace image_transport
+{
 
-  auto node = rclcpp::Node::make_shared("image_republisher");
+Republisher::Republisher(const rclcpp::NodeOptions & options)
+: Node("point_cloud_republisher", options)
+{
+  // Initialize Republishercomponent after construction
+  // shared_from_this can't be used in the constructor
+  this->timer_ = create_wall_timer(
+    1ms, [this]() {
+      if (initialized_) {
+        timer_->cancel();
+      } else {
+        this->initialize();
+        initialized_ = true;
+      }
+    });
+}
 
+void Republisher::initialize()
+{
   std::string in_topic = rclcpp::expand_topic_or_service_name(
     "in",
-    node->get_name(), node->get_namespace());
+    this->get_name(), this->get_namespace());
   std::string out_topic = rclcpp::expand_topic_or_service_name(
     "out",
-    node->get_name(), node->get_namespace());
+    this->get_name(), this->get_namespace());
+
+  std::string in_transport = "raw";
+  this->declare_parameter<std::string>("in_transport", in_transport);
+  if (!this->get_parameter(
+      "in_transport", in_transport))
+  {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(),
+      "The 'in_transport' parameter was not defined." << in_transport);
+  } else {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      "The 'in_transport' parameter is set to: " << in_transport);
+  }
 
-  std::string in_transport = vargv[1];
+  std::string out_transport = "";
+  this->declare_parameter<std::string>("out_transport", out_transport);
+  if (!this->get_parameter(
+      "out_transport", out_transport))
+  {
+    RCLCPP_WARN_STREAM(
+      this->get_logger(),
+      "The parameter 'out_transport' was not defined." << out_transport);
+  } else {
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      "The 'out_transport' parameter is set to: " << out_transport);
+  }
 
-  if (vargv.size() < 3) {
+  if (out_transport.empty()) {
     // Use all available transports for output
     rclcpp::PublisherOptions pub_options;
     auto qos_override_options = rclcpp::QosOverridingOptions(
-    {
-      rclcpp::QosPolicyKind::Depth,
-      rclcpp::QosPolicyKind::Durability,
-      rclcpp::QosPolicyKind::History,
-      rclcpp::QosPolicyKind::Reliability,
-    });
+      {
+        rclcpp::QosPolicyKind::Depth,
+        rclcpp::QosPolicyKind::Durability,
+        rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Reliability,
+      });
 
     pub_options.qos_overriding_options = qos_override_options;
-    auto pub = image_transport::create_publisher(
-      node.get(), out_topic,
+    this->pub = image_transport::create_publisher(
+      this, out_topic,
       rmw_qos_profile_default, pub_options);
 
     // Use Publisher::publish as the subscriber callback
@@ -83,31 +120,35 @@ int main(int argc, char ** argv)
     rclcpp::SubscriptionOptions sub_options;
     sub_options.qos_overriding_options = qos_override_options;
 
-    auto sub = image_transport::create_subscription(
-      node.get(), in_topic, std::bind(pub_mem_fn, &pub, std::placeholders::_1),
+    this->sub = image_transport::create_subscription(
+      this, in_topic, std::bind(pub_mem_fn, &pub, std::placeholders::_1),
       in_transport, rmw_qos_profile_default, sub_options);
-    rclcpp::spin(node);
   } else {
     // Use one specific transport for output
-    std::string out_transport = vargv[2];
-
     // Load transport plugin
     typedef image_transport::PublisherPlugin Plugin;
-    pluginlib::ClassLoader<Plugin> loader("image_transport", "image_transport::PublisherPlugin");
+    loader = std::make_shared<pluginlib::ClassLoader<Plugin>>(
+      "image_transport",
+      "image_transport::PublisherPlugin");
     std::string lookup_name = Plugin::getLookupName(out_transport);
 
-    auto instance = loader.createUniqueInstance(lookup_name);
-    std::shared_ptr<Plugin> pub = std::move(instance);
-    pub->advertise(node.get(), out_topic);
+    instance = loader->createUniqueInstance(lookup_name);
+    instance->advertise(this, out_topic);
 
     // Use PublisherPlugin::publish as the subscriber callback
     typedef void (Plugin::* PublishMemFn)(const sensor_msgs::msg::Image::ConstSharedPtr &) const;
     PublishMemFn pub_mem_fn = &Plugin::publishPtr;
-    auto sub = image_transport::create_subscription(
-      node.get(), in_topic,
-      std::bind(pub_mem_fn, pub.get(), std::placeholders::_1), in_transport);
-    rclcpp::spin(node);
+    this->sub = image_transport::create_subscription(
+      this, in_topic,
+      std::bind(pub_mem_fn, instance.get(), std::placeholders::_1), in_transport);
   }
-
-  return 0;
 }
+
+}  // namespace image_transport
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(image_transport::Republisher)