From 67c7a760a96fa768032ec3b9376fabf22b026c0c Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Thu, 18 Jan 2024 20:57:41 -0500 Subject: [PATCH] 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();