From 67c7a760a96fa768032ec3b9376fabf22b026c0c Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Thu, 18 Jan 2024 20:57:41 -0500 Subject: [PATCH 1/3] image_publisher: functional component * filename now functions when using a component * parameter callback gets called at startup when using the component * cleanup a bit of the logging --- .../image_publisher/image_publisher.hpp | 3 +- image_publisher/src/image_publisher.cpp | 28 +++++++++++-------- image_publisher/src/image_publisher_node.cpp | 3 +- 3 files changed, 19 insertions(+), 15 deletions(-) diff --git a/image_publisher/include/image_publisher/image_publisher.hpp b/image_publisher/include/image_publisher/image_publisher.hpp index 082b5d985..ef2ca4316 100644 --- a/image_publisher/include/image_publisher/image_publisher.hpp +++ b/image_publisher/include/image_publisher/image_publisher.hpp @@ -46,7 +46,8 @@ namespace image_publisher class ImagePublisher : public rclcpp::Node { public: - explicit ImagePublisher(const rclcpp::NodeOptions & options); + explicit ImagePublisher(const rclcpp::NodeOptions & options, + const std::string & filename = ""); protected: void onInit(); diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index f42aba575..886bf524b 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -49,7 +49,8 @@ namespace image_publisher using namespace std::chrono_literals; -ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options) +ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options, + const std::string & filename) : rclcpp::Node("ImagePublisher", options) { // For compressed topics to remap appropriately, we need to pass a @@ -69,41 +70,44 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options) auto param_change_callback = [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult { - RCLCPP_INFO(get_logger(), "param_change_callback"); + RCLCPP_INFO(get_logger(), "Updating parameters:"); auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; for (auto parameter : parameters) { if (parameter.get_name() == "filename") { filename_ = parameter.as_string(); - RCLCPP_INFO(get_logger(), "Reset filename as '%s'", filename_.c_str()); + RCLCPP_INFO(get_logger(), "Update filename to '%s'", filename_.c_str()); ImagePublisher::onInit(); return result; } else if (parameter.get_name() == "flip_horizontal") { flip_horizontal_ = parameter.as_bool(); - RCLCPP_INFO(get_logger(), "Reset flip_horizontal as '%d'", flip_horizontal_); + RCLCPP_INFO(get_logger(), "Update flip_horizontal to '%d'", flip_horizontal_); ImagePublisher::onInit(); return result; } else if (parameter.get_name() == "flip_vertical") { flip_vertical_ = parameter.as_bool(); - RCLCPP_INFO(get_logger(), "Reset flip_vertical as '%d'", flip_vertical_); + RCLCPP_INFO(get_logger(), "Update flip_vertical to '%d'", flip_vertical_); ImagePublisher::onInit(); return result; } else if (parameter.get_name() == "frame_id") { frame_id_ = parameter.as_string(); - RCLCPP_INFO(get_logger(), "Reset frame_id as '%s'", frame_id_.c_str()); + RCLCPP_INFO(get_logger(), "Update frame_id to '%s'", frame_id_.c_str()); } else if (parameter.get_name() == "publish_rate") { publish_rate_ = parameter.as_double(); - RCLCPP_INFO(get_logger(), "Reset publish_rate as '%lf'", publish_rate_); + RCLCPP_INFO(get_logger(), "Update publish_rate to '%lf'", publish_rate_); } else if (parameter.get_name() == "camera_info_url") { camera_info_url_ = parameter.as_string(); - RCLCPP_INFO(get_logger(), "Reset camera_info_rul as '%s'", camera_info_url_.c_str()); + RCLCPP_INFO(get_logger(), "Update camera_info_url to '%s'", camera_info_url_.c_str()); } } ImagePublisher::reconfigureCallback(); return result; }; on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback); + + // Set the filename after we do add_on_set_parameters_callback so the callback triggers + filename_ = this->declare_parameter("filename", filename); } void ImagePublisher::reconfigureCallback() @@ -114,7 +118,7 @@ void ImagePublisher::reconfigureCallback() camera_info_manager::CameraInfoManager c(this); if (!camera_info_url_.empty()) { - RCLCPP_INFO(get_logger(), "camera_info_url exist"); + RCLCPP_INFO(get_logger(), "camera_info_url: %s", camera_info_url_.c_str()); try { c.validateURL(camera_info_url_); c.loadCameraInfo(camera_info_url_); @@ -163,7 +167,7 @@ void ImagePublisher::doWork() void ImagePublisher::onInit() { - RCLCPP_INFO(this->get_logger(), "File name for publishing image is : %s", filename_.c_str()); + RCLCPP_INFO(this->get_logger(), "File name for publishing image is: %s", filename_.c_str()); try { image_ = cv::imread(filename_, cv::IMREAD_COLOR); if (image_.empty()) { // if filename not exist, open video device @@ -192,10 +196,10 @@ void ImagePublisher::onInit() RCLCPP_INFO( this->get_logger(), - "Flip horizontal image is : %s", ((flip_horizontal_) ? "true" : "false")); + "Flip horizontal image is: %s", ((flip_horizontal_) ? "true" : "false")); RCLCPP_INFO( this->get_logger(), - "Flip flip_vertical image is : %s", ((flip_vertical_) ? "true" : "false")); + "Flip flip_vertical image is: %s", ((flip_vertical_) ? "true" : "false")); // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html // #void flip(InputArray src, OutputArray dst, int flipCode) diff --git a/image_publisher/src/image_publisher_node.cpp b/image_publisher/src/image_publisher_node.cpp index 39aff2dd5..6667fd8c7 100644 --- a/image_publisher/src/image_publisher_node.cpp +++ b/image_publisher/src/image_publisher_node.cpp @@ -49,8 +49,7 @@ int main(int argc, char ** argv) } rclcpp::NodeOptions options; - auto publisher = std::make_shared(options); - publisher->declare_parameter("filename", argv[1]); + auto publisher = std::make_shared(options, argv[1]); rclcpp::spin(publisher); rclcpp::shutdown(); From 073ae2b0902fe43c5bcce96fd379bba0df6df9c1 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Thu, 18 Jan 2024 21:13:14 -0500 Subject: [PATCH 2/3] add launch file to test component --- .../image_publisher_component_file.launch.py | 73 +++++++++++++++++++ 1 file changed, 73 insertions(+) create mode 100644 image_publisher/launch/image_publisher_component_file.launch.py diff --git a/image_publisher/launch/image_publisher_component_file.launch.py b/image_publisher/launch/image_publisher_component_file.launch.py new file mode 100644 index 000000000..5504a68cb --- /dev/null +++ b/image_publisher/launch/image_publisher_component_file.launch.py @@ -0,0 +1,73 @@ +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +# This is basically the same as image_publsher_file.launch.py - but using the component +def generate_launch_description(): + filename = os.path.join(get_package_share_directory('image_publisher'), 'launch', + 'splash.png') + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + return LaunchDescription([ + + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation clock if true'), + + ComposableNodeContainer( + name='image_publisher_container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='image_publisher', + plugin='image_publisher::ImagePublisher', + name='image_publisher', + parameters=[{'filename': filename, + 'use_sim_time': use_sim_time}], + remappings=[('image_raw', '/camera/image_raw'), + ('camera_info', '/camera/camera_info')], + ) + ], + output='screen', + ), + ]) From add48dbb6444de0e2123a68dee7ae02adc0af7a4 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Thu, 18 Jan 2024 21:36:37 -0500 Subject: [PATCH 3/3] linting, revert some changes to logging --- .../image_publisher/image_publisher.hpp | 5 +++-- image_publisher/src/image_publisher.cpp | 19 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/image_publisher/include/image_publisher/image_publisher.hpp b/image_publisher/include/image_publisher/image_publisher.hpp index ef2ca4316..6c22b0199 100644 --- a/image_publisher/include/image_publisher/image_publisher.hpp +++ b/image_publisher/include/image_publisher/image_publisher.hpp @@ -46,8 +46,9 @@ namespace image_publisher class ImagePublisher : public rclcpp::Node { public: - explicit ImagePublisher(const rclcpp::NodeOptions & options, - const std::string & filename = ""); + ImagePublisher( + const rclcpp::NodeOptions & options, + const std::string & filename = ""); protected: void onInit(); diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index 886bf524b..1f8068de4 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -49,8 +49,9 @@ namespace image_publisher using namespace std::chrono_literals; -ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options, - const std::string & filename) +ImagePublisher::ImagePublisher( + const rclcpp::NodeOptions & options, + const std::string & filename) : rclcpp::Node("ImagePublisher", options) { // For compressed topics to remap appropriately, we need to pass a @@ -70,35 +71,33 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options, auto param_change_callback = [this](std::vector parameters) -> rcl_interfaces::msg::SetParametersResult { - RCLCPP_INFO(get_logger(), "Updating parameters:"); - auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; for (auto parameter : parameters) { if (parameter.get_name() == "filename") { filename_ = parameter.as_string(); - RCLCPP_INFO(get_logger(), "Update filename to '%s'", filename_.c_str()); + RCLCPP_INFO(get_logger(), "Reset filename as '%s'", filename_.c_str()); ImagePublisher::onInit(); return result; } else if (parameter.get_name() == "flip_horizontal") { flip_horizontal_ = parameter.as_bool(); - RCLCPP_INFO(get_logger(), "Update flip_horizontal to '%d'", flip_horizontal_); + RCLCPP_INFO(get_logger(), "Reset flip_horizontal as '%d'", flip_horizontal_); ImagePublisher::onInit(); return result; } else if (parameter.get_name() == "flip_vertical") { flip_vertical_ = parameter.as_bool(); - RCLCPP_INFO(get_logger(), "Update flip_vertical to '%d'", flip_vertical_); + RCLCPP_INFO(get_logger(), "Reset flip_vertical as '%d'", flip_vertical_); ImagePublisher::onInit(); return result; } else if (parameter.get_name() == "frame_id") { frame_id_ = parameter.as_string(); - RCLCPP_INFO(get_logger(), "Update frame_id to '%s'", frame_id_.c_str()); + RCLCPP_INFO(get_logger(), "Reset frame_id as '%s'", frame_id_.c_str()); } else if (parameter.get_name() == "publish_rate") { publish_rate_ = parameter.as_double(); - RCLCPP_INFO(get_logger(), "Update publish_rate to '%lf'", publish_rate_); + RCLCPP_INFO(get_logger(), "Reset publish_rate as '%lf'", publish_rate_); } else if (parameter.get_name() == "camera_info_url") { camera_info_url_ = parameter.as_string(); - RCLCPP_INFO(get_logger(), "Update camera_info_url to '%s'", camera_info_url_.c_str()); + RCLCPP_INFO(get_logger(), "Reset camera_info_url as '%s'", camera_info_url_.c_str()); } } ImagePublisher::reconfigureCallback();