From 38cc6702f11068dc8f66122bdcfef8ba385b2095 Mon Sep 17 00:00:00 2001
From: mhubii <m.huber_1994@hotmail.de>
Date: Thu, 2 May 2024 19:10:56 +0200
Subject: [PATCH] image_proc::CropDecimateNode: parameter handle (#967)

---
 .../include/image_proc/crop_decimate.hpp      |  5 ++
 image_proc/src/crop_decimate.cpp              | 51 +++++++++++++++++++
 2 files changed, 56 insertions(+)

diff --git a/image_proc/include/image_proc/crop_decimate.hpp b/image_proc/include/image_proc/crop_decimate.hpp
index de9a7d65d..f57f871ea 100644
--- a/image_proc/include/image_proc/crop_decimate.hpp
+++ b/image_proc/include/image_proc/crop_decimate.hpp
@@ -34,10 +34,12 @@
 #define IMAGE_PROC__CROP_DECIMATE_HPP_
 
 #include <string>
+#include <vector>
 
 #include "cv_bridge/cv_bridge.hpp"
 
 #include <image_transport/image_transport.hpp>
+#include <rcl_interfaces/msg/set_parameters_result.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/image.hpp>
@@ -66,6 +68,7 @@ class CropDecimateNode : public rclcpp::Node
 private:
   image_transport::CameraSubscriber sub_;
   image_transport::CameraPublisher pub_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_handle_;
   int queue_size_;
   std::string target_frame_id_;
   int decimation_x_, decimation_y_, offset_x_, offset_y_, width_, height_;
@@ -75,6 +78,8 @@ class CropDecimateNode : public rclcpp::Node
   void imageCb(
     const sensor_msgs::msg::Image::ConstSharedPtr image_msg,
     const sensor_msgs::msg::CameraInfo::ConstSharedPtr info_msg);
+
+  rcl_interfaces::msg::SetParametersResult paramCb(const std::vector<rclcpp::Parameter> &);
 };
 
 }  // namespace image_proc
diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp
index a53ab9ac2..810443a5f 100644
--- a/image_proc/src/crop_decimate.cpp
+++ b/image_proc/src/crop_decimate.cpp
@@ -158,6 +158,10 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
   // Create publisher with QoS matched to subscribed topic publisher
   auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_ = image_transport::create_camera_publisher(this, pub_topic, qos_profile, pub_options);
+
+  // Create parameter callback handle
+  param_cb_handle_ = this->add_on_set_parameters_callback(
+    std::bind(&CropDecimateNode::paramCb, this, std::placeholders::_1));
 }
 
 void CropDecimateNode::imageCb(
@@ -356,6 +360,53 @@ void CropDecimateNode::imageCb(
   pub_.publish(out_image, out_info);
 }
 
+rcl_interfaces::msg::SetParametersResult CropDecimateNode::paramCb(
+  const std::vector<rclcpp::Parameter>& parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  result.successful = true;
+  for (const auto & param : parameters) {
+    if (param.get_name() == "offset_x") {
+      if (param.as_int() < 0) {
+        result.reason = "offset_x must be non-negative";
+        RCLCPP_WARN(get_logger(), result.reason.c_str());
+        result.successful = false;
+        continue;
+      }
+      offset_x_ = param.as_int();
+      RCLCPP_INFO(get_logger(), "New offset_x: %d", offset_x_);
+    } else if (param.get_name() == "offset_y") {
+      if (param.as_int() < 0) {
+        result.reason = "offset_y must be non-negative";
+        RCLCPP_WARN(get_logger(), result.reason.c_str());
+        result.successful = false;
+        continue;
+      }
+      offset_y_ = param.as_int();
+      RCLCPP_INFO(get_logger(), "New offset_y: %d", offset_y_);
+    } else if (param.get_name() == "width") {
+      if (param.as_int() < 0) {
+        result.reason = "width must be non-negative";
+        RCLCPP_WARN(get_logger(), result.reason.c_str());
+        result.successful = false;
+        continue;
+      }
+      width_ = param.as_int();
+      RCLCPP_INFO(get_logger(), "New width: %d", width_);
+    } else if (param.get_name() == "height") {
+      if (param.as_int() < 0) {
+        result.reason = "height must be non-negative";
+        RCLCPP_WARN(get_logger(), result.reason.c_str());
+        result.successful = false;
+        continue;
+      }
+      height_ = param.as_int();
+      RCLCPP_INFO(get_logger(), "New height: %d", height_);
+    }
+  }
+  return result;
+}
+
 }  // namespace image_proc
 
 #include "rclcpp_components/register_node_macro.hpp"