diff --git a/aruco_opencv/src/aruco_tracker.cpp b/aruco_opencv/src/aruco_tracker.cpp index 041a7d2..b5a7f4d 100644 --- a/aruco_opencv/src/aruco_tracker.cpp +++ b/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::SharedPtr detection_pub_; rclcpp_lifecycle::LifecyclePublisher::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 & 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()