diff --git a/CMakeLists.txt b/CMakeLists.txt index 70d3befb..791070e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ endif() find_package(OpenCV REQUIRED) find_package(PkgConfig REQUIRED) +pkg_check_modules(avcodec REQUIRED libavcodec) pkg_check_modules(avutil REQUIRED libavutil) pkg_check_modules(swscale REQUIRED libswscale) diff --git a/include/usb_cam/formats/av_pixel_format_helper.hpp b/include/usb_cam/formats/av_pixel_format_helper.hpp index 33d8dfd6..4b0db755 100644 --- a/include/usb_cam/formats/av_pixel_format_helper.hpp +++ b/include/usb_cam/formats/av_pixel_format_helper.hpp @@ -38,6 +38,8 @@ extern "C" { #include "libavutil/pixfmt.h" } +#include "usb_cam/constants.hpp" + #define stringify(name) #name @@ -958,6 +960,152 @@ inline AVPixelFormat get_av_pixel_format_from_string(const std::string & str) return STR_2_AVPIXFMT.find(fullFmtStr)->second; } +/// @brief Get ROS PixelFormat from AVPixelFormat. +/// @param avPixelFormat AVPixelFormat +/// @return String specifying the ROS pixel format. +inline std::string get_ros_pixel_format_from_av_format(const AVPixelFormat & avPixelFormat) +{ + std::string ros_format = ""; + + switch (avPixelFormat) { + default: + { + ros_format = usb_cam::constants::UNKNOWN; + } + break; + + case AVPixelFormat::AV_PIX_FMT_RGB24: + { + ros_format = usb_cam::constants::RGB8; + } + break; + + case AVPixelFormat::AV_PIX_FMT_RGBA: + { + ros_format = usb_cam::constants::RGBA8; + } + break; + + case AVPixelFormat::AV_PIX_FMT_BGR24: + { + ros_format = usb_cam::constants::BGR8; + } + break; + + case AVPixelFormat::AV_PIX_FMT_BGRA: + { + ros_format = usb_cam::constants::BGRA8; + } + break; + + case AVPixelFormat::AV_PIX_FMT_GRAY8: + { + ros_format = usb_cam::constants::MONO8; + } + break; + + case AVPixelFormat::AV_PIX_FMT_GRAY16BE: + { + ros_format = usb_cam::constants::MONO16; + } + break; + + case AVPixelFormat::AV_PIX_FMT_YUV422P: + { + ros_format = usb_cam::constants::YUV422; + } + break; + + case AVPixelFormat::AV_PIX_FMT_YUV420P: + { + ros_format = usb_cam::constants::NV21; + } + break; + + case AVPixelFormat::AV_PIX_FMT_YUV444P: + { + ros_format = usb_cam::constants::NV24; + } + break; + } + + return ros_format; +} + +/// @brief Get the number of channels from AVPixelFormat. +/// @param avPixelFormat AVPixelFormat +/// @return Number of channels as uint8 +inline uint8_t get_channels_from_av_format(const AVPixelFormat & avPixelFormat) +{ + uint8_t channels = 1; + + switch (avPixelFormat) { + default: + case AVPixelFormat::AV_PIX_FMT_GRAY8: + case AVPixelFormat::AV_PIX_FMT_GRAY16BE: + { + channels = 1; + } + break; + + case AVPixelFormat::AV_PIX_FMT_YUV422P: + case AVPixelFormat::AV_PIX_FMT_YUV420P: + case AVPixelFormat::AV_PIX_FMT_YUV444P: + { + channels = 2; + } + break; + + case AVPixelFormat::AV_PIX_FMT_RGB24: + case AVPixelFormat::AV_PIX_FMT_BGR24: + { + channels = 3; + } + break; + + case AVPixelFormat::AV_PIX_FMT_RGBA: + case AVPixelFormat::AV_PIX_FMT_BGRA: + { + channels = 4; + } + break; + } + + return channels; +} + +/// @brief Get the pixel bit depth from AVPixelFormat. +/// @param avPixelFormat AVPixelFormat +/// @return Bit depth as uint8 +inline uint8_t get_bit_depth_from_av_format(const AVPixelFormat & avPixelFormat) +{ + uint8_t bit_depth = 8; + + switch (avPixelFormat) { + default: + case AVPixelFormat::AV_PIX_FMT_GRAY8: + case AVPixelFormat::AV_PIX_FMT_RGB24: + case AVPixelFormat::AV_PIX_FMT_BGR24: + case AVPixelFormat::AV_PIX_FMT_RGBA: + case AVPixelFormat::AV_PIX_FMT_BGRA: + case AVPixelFormat::AV_PIX_FMT_YUV422P: + case AVPixelFormat::AV_PIX_FMT_YUV420P: + case AVPixelFormat::AV_PIX_FMT_YUV444P: + { + bit_depth = 8; + } + break; + + case AVPixelFormat::AV_PIX_FMT_GRAY16BE: + { + bit_depth = 16; + } + break; + } + + return bit_depth; +} + } // namespace formats } // namespace usb_cam diff --git a/include/usb_cam/formats/mjpeg.hpp b/include/usb_cam/formats/mjpeg.hpp index ba5582de..da3b4b15 100644 --- a/include/usb_cam/formats/mjpeg.hpp +++ b/include/usb_cam/formats/mjpeg.hpp @@ -52,6 +52,7 @@ extern "C" { #include "usb_cam/usb_cam.hpp" #include "usb_cam/formats/pixel_format_base.hpp" #include "usb_cam/formats/utils.hpp" +#include "usb_cam/formats/av_pixel_format_helper.hpp" namespace usb_cam @@ -59,6 +60,21 @@ namespace usb_cam namespace formats { +class RAW_MJPEG : public pixel_format_base +{ +public: + explicit RAW_MJPEG(const AVPixelFormat & avDeviceFormat) + : pixel_format_base( + "raw_mjpeg", + V4L2_PIX_FMT_MJPEG, + get_ros_pixel_format_from_av_format(avDeviceFormat), + get_channels_from_av_format(avDeviceFormat), + get_bit_depth_from_av_format(avDeviceFormat), + false) + { + } +}; + class MJPEG2RGB : public pixel_format_base { public: diff --git a/include/usb_cam/usb_cam.hpp b/include/usb_cam/usb_cam.hpp index babbc3d0..3568b350 100644 --- a/include/usb_cam/usb_cam.hpp +++ b/include/usb_cam/usb_cam.hpp @@ -288,6 +288,7 @@ class UsbCam using usb_cam::formats::MONO8; using usb_cam::formats::MONO16; using usb_cam::formats::Y102MONO8; + using usb_cam::formats::RAW_MJPEG; using usb_cam::formats::MJPEG2RGB; using usb_cam::formats::M4202RGB; @@ -303,6 +304,9 @@ class UsbCam } else if (parameters.pixel_format_name == "uyvy2rgb") { // number of pixels required for conversion method m_image.pixel_format = std::make_shared(m_image.number_of_pixels); + } else if (parameters.pixel_format_name == "mjpeg") { + m_image.pixel_format = std::make_shared( + formats::get_av_pixel_format_from_string(parameters.av_device_format)); } else if (parameters.pixel_format_name == "mjpeg2rgb") { m_image.pixel_format = std::make_shared( m_image.width, m_image.height, diff --git a/include/usb_cam/usb_cam_node.hpp b/include/usb_cam/usb_cam_node.hpp index a26ed6a9..5de9579b 100644 --- a/include/usb_cam/usb_cam_node.hpp +++ b/include/usb_cam/usb_cam_node.hpp @@ -39,6 +39,7 @@ #include "image_transport/image_transport.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/compressed_image.hpp" #include "std_srvs/srv/set_bool.hpp" #include "usb_cam/usb_cam.hpp" @@ -66,6 +67,7 @@ class UsbCamNode : public rclcpp::Node void set_v4l2_params(); void update(); bool take_and_send_image(); + bool take_and_send_image_mjpeg(); rcl_interfaces::msg::SetParametersResult parameters_callback( const std::vector & parameters); @@ -77,8 +79,11 @@ class UsbCamNode : public rclcpp::Node UsbCam * m_camera; - sensor_msgs::msg::Image::SharedPtr m_image_msg; - image_transport::CameraPublisher m_image_publisher; + sensor_msgs::msg::Image::UniquePtr m_image_msg; + sensor_msgs::msg::CompressedImage::UniquePtr m_compressed_img_msg; + std::shared_ptr m_image_publisher; + rclcpp::Publisher::SharedPtr m_compressed_image_publisher; + rclcpp::Publisher::SharedPtr m_compressed_cam_info_publisher; parameters_t m_parameters; diff --git a/include/usb_cam/utils.hpp b/include/usb_cam/utils.hpp index ee404646..b847b4f2 100644 --- a/include/usb_cam/utils.hpp +++ b/include/usb_cam/utils.hpp @@ -35,6 +35,7 @@ extern "C" { #include #include +#include // for access #include #include #include @@ -47,6 +48,7 @@ extern "C" { #include "usb_cam/constants.hpp" #include "usb_cam/formats/pixel_format_base.hpp" + namespace usb_cam { namespace utils diff --git a/src/ros2/usb_cam_node.cpp b/src/ros2/usb_cam_node.cpp index 579d0207..0133ba31 100644 --- a/src/ros2/usb_cam_node.cpp +++ b/src/ros2/usb_cam_node.cpp @@ -34,6 +34,7 @@ #include "usb_cam/usb_cam_node.hpp" #include "usb_cam/utils.hpp" +const char BASE_TOPIC_NAME[] = "image_raw"; namespace usb_cam { @@ -42,6 +43,12 @@ UsbCamNode::UsbCamNode(const rclcpp::NodeOptions & node_options) : Node("usb_cam", node_options), m_camera(new usb_cam::UsbCam()), m_image_msg(new sensor_msgs::msg::Image()), + m_compressed_img_msg(nullptr), + m_image_publisher(std::make_shared( + image_transport::create_camera_publisher(this, BASE_TOPIC_NAME, + rclcpp::QoS {100}.get_rmw_qos_profile()))), + m_compressed_image_publisher(nullptr), + m_compressed_cam_info_publisher(nullptr), m_parameters(), m_camera_info_msg(new sensor_msgs::msg::CameraInfo()), m_service_capture( @@ -54,10 +61,6 @@ UsbCamNode::UsbCamNode(const rclcpp::NodeOptions & node_options) std::placeholders::_2, std::placeholders::_3))) { - m_image_publisher = image_transport::create_camera_publisher( - this, "image_raw", rclcpp::QoS {100}.get_rmw_qos_profile() - ); - // declare params this->declare_parameter("camera_name", "default_cam"); this->declare_parameter("camera_info_url", ""); @@ -93,6 +96,7 @@ UsbCamNode::~UsbCamNode() { RCLCPP_WARN(this->get_logger(), "Shutting down"); m_image_msg.reset(); + m_compressed_img_msg.reset(); m_camera_info_msg.reset(); m_camera_info.reset(); m_timer.reset(); @@ -144,8 +148,8 @@ void UsbCamNode::init() if (available_devices.find(m_parameters.device_name) == available_devices.end()) { RCLCPP_ERROR_STREAM( this->get_logger(), - "Device specified is not available or is not a vaild V4L2 device: `" - << m_parameters.device_name << "`" + "Device specified is not available or is not a vaild V4L2 device: `" << + m_parameters.device_name << "`" ); RCLCPP_INFO(this->get_logger(), "Available V4L2 devices are:"); for (const auto & device : available_devices) { @@ -156,6 +160,19 @@ void UsbCamNode::init() return; } + // if pixel format is equal to 'mjpeg', i.e. raw mjpeg stream, initialize compressed image message + // and publisher + if (m_parameters.pixel_format_name == "mjpeg") { + m_compressed_img_msg.reset(new sensor_msgs::msg::CompressedImage()); + m_compressed_img_msg->header.frame_id = m_parameters.frame_id; + m_compressed_image_publisher = + this->create_publisher( + std::string(BASE_TOPIC_NAME) + "/compressed", rclcpp::QoS(100)); + m_compressed_cam_info_publisher = + this->create_publisher( + "camera_info", rclcpp::QoS(100)); + } + m_image_msg->header.frame_id = m_parameters.frame_id; RCLCPP_INFO( this->get_logger(), "Starting '%s' (%s) at %dx%d via %s (%s) at %i FPS", @@ -361,7 +378,30 @@ bool UsbCamNode::take_and_send_image() *m_camera_info_msg = m_camera_info->getCameraInfo(); m_camera_info_msg->header = m_image_msg->header; - m_image_publisher.publish(*m_image_msg, *m_camera_info_msg); + m_image_publisher->publish(*m_image_msg, *m_camera_info_msg); + return true; +} + +bool UsbCamNode::take_and_send_image_mjpeg() +{ + // Only resize if required + if (sizeof(m_compressed_img_msg->data) != m_camera->get_image_size_in_bytes()) { + m_compressed_img_msg->format = "jpeg"; + m_compressed_img_msg->data.resize(m_camera->get_image_size_in_bytes()); + } + + // grab the image, pass image msg buffer to fill + m_camera->get_image(reinterpret_cast(&m_compressed_img_msg->data[0])); + + auto stamp = m_camera->get_image_timestamp(); + m_compressed_img_msg->header.stamp.sec = stamp.tv_sec; + m_compressed_img_msg->header.stamp.nanosec = stamp.tv_nsec; + + *m_camera_info_msg = m_camera_info->getCameraInfo(); + m_camera_info_msg->header = m_compressed_img_msg->header; + + m_compressed_image_publisher->publish(*m_compressed_img_msg); + m_compressed_cam_info_publisher->publish(*m_camera_info_msg); return true; } @@ -382,8 +422,12 @@ void UsbCamNode::update() { if (m_camera->is_capturing()) { // If the camera exposure longer higher than the framerate period - // then that caps the framerate - if (!take_and_send_image()) { + // then that caps the framerate. + // auto t0 = now(); + bool isSuccessful = (m_parameters.pixel_format_name == "mjpeg") ? + take_and_send_image_mjpeg() : + take_and_send_image(); + if (!isSuccessful) { RCLCPP_WARN_ONCE(this->get_logger(), "USB camera did not respond in time."); } }