Use post-set parameters callback (#41) (#42)

(cherry picked from commit c1926ed)

Co-authored-by: Błażej Sowa <bsowa123@gmail.com>

aruco_opencv/src/aruco_tracker.cpp

@@ -68,6 +68,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
 
   // ROS
   OnSetParametersCallbackHandle::SharedPtr on_set_parameter_callback_handle_;
+  PostSetParametersCallbackHandle::SharedPtr post_set_parameter_callback_handle_;
   rclcpp_lifecycle::LifecyclePublisher<aruco_opencv_msgs::msg::ArucoDetection>::SharedPtr
     detection_pub_;
   rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>::SharedPtr debug_pub_;
@@ -160,11 +161,10 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
     detection_pub_->on_activate();
     debug_pub_->on_activate();
 
-    on_set_parameter_callback_handle_ =
-      add_on_set_parameters_callback(
-      std::bind(
-        &ArucoTracker::callback_on_set_parameters,
-        this, std::placeholders::_1));
+    on_set_parameter_callback_handle_ = add_on_set_parameters_callback(
+      std::bind(&ArucoTracker::callback_on_set_parameters, this, std::placeholders::_1));
+    post_set_parameter_callback_handle_ = add_post_set_parameters_callback(
+      std::bind(&ArucoTracker::callback_post_set_parameters, this, std::placeholders::_1));
 
     RCLCPP_INFO(get_logger(), "Waiting for first camera info...");
 
@@ -204,6 +204,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
     RCLCPP_INFO(get_logger(), "Deactivating");
 
     on_set_parameter_callback_handle_.reset();
+    post_set_parameter_callback_handle_.reset();
     cam_info_sub_.reset();
     img_sub_.reset();
     compressed_img_sub_.reset();
@@ -235,6 +236,7 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
     RCLCPP_INFO(get_logger(), "Shutting down");
 
     on_set_parameter_callback_handle_.reset();
+    post_set_parameter_callback_handle_.reset();
     cam_info_sub_.reset();
     img_sub_.reset();
     compressed_img_sub_.reset();
@@ -328,6 +330,11 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
       }
     }
 
+    return result;
+  }
+
+  void callback_post_set_parameters(const std::vector<rclcpp::Parameter> & parameters)
+  {
     bool aruco_param_changed = false;
     for (auto & param : parameters) {
       if (param.get_name() == "marker_size") {
@@ -345,13 +352,9 @@ class ArucoTracker : public rclcpp_lifecycle::LifecycleNode
         "Parameter \"" << param.get_name() << "\" changed to " << param.value_to_string());
     }
 
-    if (!aruco_param_changed) {
-      return result;
+    if (aruco_param_changed) {
+      retrieve_aruco_parameters(*this, detector_parameters_);
     }
-
-    retrieve_aruco_parameters(*this, detector_parameters_);
-
-    return result;
   }
 
   void load_boards()