From 1a559edc0587dec064d095f0a711405c4d7dfb1b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 20 Feb 2024 09:59:06 +0100 Subject: [PATCH] Added rclcpp component to Republish (#275) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- image_transport/CMakeLists.txt | 30 ++++- .../include/image_transport/republish.hpp | 62 +++++++++ image_transport/package.xml | 1 + image_transport/src/republish.cpp | 123 ++++++++++++------ 4 files changed, 170 insertions(+), 46 deletions(-) create mode 100644 image_transport/include/image_transport/republish.hpp diff --git a/image_transport/CMakeLists.txt b/image_transport/CMakeLists.txt index 7b658bd9..089d5a12 100644 --- a/image_transport/CMakeLists.txt +++ b/image_transport/CMakeLists.txt @@ -17,6 +17,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 @@ -58,11 +59,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) @@ -78,10 +92,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}) diff --git a/image_transport/include/image_transport/republish.hpp b/image_transport/include/image_transport/republish.hpp new file mode 100644 index 00000000..11fb7f43 --- /dev/null +++ b/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 + +#include "image_transport/image_transport.hpp" +#include "image_transport/visibility_control.hpp" + +#include + +#include + +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 instance; + std::shared_ptr> loader; +}; + +} // namespace image_transport +#endif // IMAGE_TRANSPORT__REPUBLISH_HPP_ diff --git a/image_transport/package.xml b/image_transport/package.xml index a30b3e3a..96962514 100644 --- a/image_transport/package.xml +++ b/image_transport/package.xml @@ -26,6 +26,7 @@ message_filters pluginlib rclcpp + rclcpp_components sensor_msgs ament_cmake_gtest diff --git a/image_transport/src/republish.cpp b/image_transport/src/republish.cpp index 394bd701..499b7cf6 100644 --- a/image_transport/src/republish.cpp +++ b/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 +#include "image_transport/republish.hpp" + +#include #include -#include -#include "pluginlib/class_loader.hpp" +#include -#include "rclcpp/rclcpp.hpp" +#include #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:= [out_transport] out:=\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("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("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 loader("image_transport", "image_transport::PublisherPlugin"); + loader = std::make_shared>( + "image_transport", + "image_transport::PublisherPlugin"); std::string lookup_name = Plugin::getLookupName(out_transport); - auto instance = loader.createUniqueInstance(lookup_name); - std::shared_ptr 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)