diff --git a/image_publisher/doc/components.rst b/image_publisher/doc/components.rst index afd7fcf5e..e381edfc2 100644 --- a/image_publisher/doc/components.rst +++ b/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 diff --git a/image_publisher/include/image_publisher/image_publisher.hpp b/image_publisher/include/image_publisher/image_publisher.hpp index 6c22b0199..ae31210d0 100644 --- a/image_publisher/include/image_publisher/image_publisher.hpp +++ b/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_ diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index 1f8068de4..dc6649e39 100644 --- a/image_publisher/src/image_publisher.cpp +++ b/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 #include +#include #include #include #include @@ -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(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(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::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(camera_info_.width / 2), 0, f_approx, static_cast(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(camera_info_.width / 2), 0, 0, 1, + 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(