[rolling] image_publisher: add field of view parameter

image_publisher/doc/components.rst

@@ -14,6 +14,7 @@ Published Topics
 Parameters
 ^^^^^^^^^^
  * **filename** (string, default: ""): Name of image file to be published.
+ * **field_of_view** (double, default: 0): Camera field of view (deg) used to calculate focal length for camera info topic.
  * **flip_horizontal** (bool, default: false): Flip output image horizontally.
  * **flip_vertical** (bool, default: false): Flip output image vertically.
  * **frame_id** (string, default: "camera") Frame id inserted in published

image_publisher/include/image_publisher/image_publisher.hpp

@@ -65,6 +65,7 @@ class ImagePublisher : public rclcpp::Node
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_;
 
   std::string filename_;
+  double field_of_view_;
   bool flip_horizontal_;
   bool flip_vertical_;
   bool retry_;  // If enabled will retry loading image from the filename_

image_publisher/src/image_publisher.cpp

@@ -31,7 +31,9 @@
 // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
+#include <cmath>
 #include <chrono>
+#include <limits>
 #include <string>
 #include <thread>
 #include <vector>
@@ -60,6 +62,7 @@ ImagePublisher::ImagePublisher(
   std::string topic_name = node_base->resolve_topic_or_service_name("image_raw", false);
   pub_ = image_transport::create_camera_publisher(this, topic_name);
 
+  field_of_view_ = this->declare_parameter("field_of_view", static_cast<double>(0));
   flip_horizontal_ = this->declare_parameter("flip_horizontal", false);
   flip_vertical_ = this->declare_parameter("flip_vertical", false);
   frame_id_ = this->declare_parameter("frame_id", std::string("camera"));
@@ -79,6 +82,11 @@ ImagePublisher::ImagePublisher(
           RCLCPP_INFO(get_logger(), "Reset filename as '%s'", filename_.c_str());
           ImagePublisher::onInit();
           return result;
+        } 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;
         } else if (parameter.get_name() == "flip_horizontal") {
           flip_horizontal_ = parameter.as_bool();
           RCLCPP_INFO(get_logger(), "Reset flip_horizontal as '%d'", flip_horizontal_);
@@ -218,10 +226,16 @@ void ImagePublisher::onInit()
   camera_info_.height = image_.rows;
   camera_info_.distortion_model = "plumb_bob";
   camera_info_.d = {0, 0, 0, 0, 0};
-  camera_info_.k = {1, 0, static_cast<float>(camera_info_.width / 2), 0, 1,
+
+  double f_approx = 1.0;  // FOV equal to 0 disables the approximation
+  if (std::abs(field_of_view_) > std::numeric_limits<double>::epsilon()) {
+    // Based on https://learnopencv.com/approximate-focal-length-for-webcams-and-cell-phone-cameras/
+    f_approx = (camera_info_.width / 2) / std::tan((field_of_view_ * M_PI / 180) / 2);
+  }
+  camera_info_.k = {f_approx, 0, static_cast<float>(camera_info_.width / 2), 0, f_approx,
     static_cast<float>(camera_info_.height / 2), 0, 0, 1};
   camera_info_.r = {1, 0, 0, 0, 1, 0, 0, 0, 1};
-  camera_info_.p = {1, 0, static_cast<float>(camera_info_.width / 2), 0, 0, 1,
+  camera_info_.p = {f_approx, 0, static_cast<float>(camera_info_.width / 2), 0, 0, f_approx,
     static_cast<float>(camera_info_.height / 2), 0, 0, 0, 1, 0};
 
   timer_ = this->create_wall_timer(