image_proc::CropDecimateNode: parameter handle (ros-perception#967)

mhubii · May 2, 2024 · 38cc670 · 38cc670
1 parent 171d436
commit 38cc670
Show file tree

Hide file tree

Showing 2 changed files with 56 additions and 0 deletions.
diff --git 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
@@ -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"