image_publisher: add field of view parameter (backport #985)

image_publisher/doc/components.rst

@@ -0,0 +1,23 @@
+Nodes and Components
+====================
+
+image_publisher::ImagePublisher
+-------------------------------
+Component to publish sensor_msgs/Image, requires filename argument.
+Also avialable as a ROS 2 node named ``image_publisher``.
+
+Published Topics
+^^^^^^^^^^^^^^^^
+ * **image_raw** (sensor_msgs/Image): ROS Image message of your input file.
+ * **camera_info** (sensor_msgs/CameraInfo): CameraInfo published along with Image.
+
+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 and camera_info.
+ * **publish_rate** (double, default: 10): Rate to publish image (hz).
+ * **camera_info_uri** (string, default: ""): Path to camera info.

image_publisher/include/image_publisher/image_publisher.hpp

@@ -63,6 +63,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 image_flipped_;

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>
@@ -54,6 +56,7 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options)
 {
   pub_ = image_transport::create_camera_publisher(this, "image_raw");
 
+  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"));
@@ -75,6 +78,11 @@ ImagePublisher::ImagePublisher(const rclcpp::NodeOptions & options)
           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_);
@@ -214,10 +222,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(