diff --git a/image_publisher/doc/components.rst b/image_publisher/doc/components.rst new file mode 100644 index 000000000..e381edfc2 --- /dev/null +++ b/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. diff --git a/image_publisher/include/image_publisher/image_publisher.hpp b/image_publisher/include/image_publisher/image_publisher.hpp index dc76bf890..278d940b6 100644 --- a/image_publisher/include/image_publisher/image_publisher.hpp +++ b/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_; diff --git a/image_publisher/src/image_publisher.cpp b/image_publisher/src/image_publisher.cpp index aac9511c5..dd90c6f2a 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 @@ -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(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(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(