Fix loading of the camera info parameters on startup

ros-perception · May 27, 2024 · d42f862 · d42f862
1 parent a87b971
commit d42f862
Showing 1 changed file with 22 additions and 11 deletions.
diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp
@@ -71,36 +71,49 @@ ImagePublisher::ImagePublisher(
   auto param_change_callback =
     [this](std::vector<rclcpp::Parameter> 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;
       for (auto parameter : parameters) {
         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() == "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_url as '%s'", camera_info_url_.c_str());
+          RCLCPP_INFO(get_logger(), "Reset camera_info_rul 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);
@@ -224,9 +237,7 @@ void ImagePublisher::onInit()
   camera_info_.p = {1, 0, static_cast<float>(camera_info_.width / 2), 0, 0, 1,
     static_cast<float>(camera_info_.height / 2), 0, 0, 0, 1, 0};
 
-  timer_ = this->create_wall_timer(
-    std::chrono::milliseconds(static_cast<int>(1000 / publish_rate_)),
-    std::bind(&ImagePublisher::doWork, this));
+  ImagePublisher::reconfigureCallback();
 }
 
 }  // namespace image_publisher