diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index dd90c6f2a..4fcf9a77e 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/image_publisher/src/image_publisher.cpp @@ -68,7 +68,8 @@ 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"); + bool call_init = false; + bool call_reconfigure = false; auto result = rcl_interfaces::msg::SetParametersResult(); result.successful = true; @@ -76,35 +77,39 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options) if (parameter.get_name() == "filename") { filename_ = parameter.as_string(); RCLCPP_INFO(get_logger(), "Reset filename as '%s'", filename_.c_str()); - ImagePublisher::onInit(); - return result; + call_init = true; } else if (parameter.get_name() == "field_of_view") { field_of_view_ = parameter.as_double(); RCLCPP_INFO(get_logger(), "Reset field_of_view as '%f'", field_of_view_); - ImagePublisher::onInit(); - return result; + call_init = true; } else if (parameter.get_name() == "flip_horizontal") { flip_horizontal_ = parameter.as_bool(); RCLCPP_INFO(get_logger(), "Reset flip_horizontal as '%d'", flip_horizontal_); - ImagePublisher::onInit(); - return result; + call_init = true; } else if (parameter.get_name() == "flip_vertical") { flip_vertical_ = parameter.as_bool(); RCLCPP_INFO(get_logger(), "Reset flip_vertical as '%d'", flip_vertical_); - ImagePublisher::onInit(); - return result; + call_init = true; } 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()); } else if (parameter.get_name() == "publish_rate") { publish_rate_ = parameter.as_double(); RCLCPP_INFO(get_logger(), "Reset publish_rate as '%lf'", publish_rate_); + call_reconfigure = true; } 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(), "Reset camera_info_url as '%s'", camera_info_url_.c_str()); + call_reconfigure = true; } } - ImagePublisher::reconfigureCallback(); + // reconfigureCallback() is called within onInit() so there is no need to call it twice + if (call_reconfigure && !call_init) { + ImagePublisher::reconfigureCallback(); + } else if (call_init) { + ImagePublisher::onInit(); + } + return result; }; on_set_parameters_callback_handle_ = this->add_on_set_parameters_callback(param_change_callback); @@ -234,9 +239,7 @@ void ImagePublisher::onInit() camera_info_.p = {f_approx, 0, static_cast(camera_info_.width / 2), 0, 0, f_approx, static_cast(camera_info_.height / 2), 0, 0, 0, 1, 0}; - timer_ = this->create_wall_timer( - std::chrono::milliseconds(static_cast(1000 / publish_rate_)), - std::bind(&ImagePublisher::doWork, this)); + ImagePublisher::reconfigureCallback(); } } // namespace image_publisher