christianrauch / camera_ros

ROS 2 node for libcamera supported cameras (V4L2, Raspberry Pi Camera Modules)
https://libcamera.org
MIT License
68 stars 30 forks source link

Invalid number of channels in input image (ov9281 camera) #56

Closed izeemeen closed 3 months ago

izeemeen commented 3 months ago

Hi all!

I use this mono camera: https://www.waveshare.com/OV9281-160-Camera.htm

After launching: ros2 launch camera_ros camera.launch.py I get this output:

[INFO] [launch]: All log files can be found below /home/lcv/.ros/log/2024-08-02-14-48-00-757628-ubuntu-5515
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [component_container-1]: process started with pid [5532]
[component_container-1] [INFO] [1722610081.325366512] [camera_container]: Load Library: /home/lcv/camera_ws/install/camera_ros/lib/libcamera_component.so
[component_container-1] [INFO] [1722610081.423067950] [camera_container]: Found class: rclcpp_components::NodeFactoryTemplate<camera::CameraNode>
[component_container-1] [INFO] [1722610081.423193562] [camera_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<camera::CameraNode>
[component_container-1] [2:50:16.467429195] [5532]  INFO Camera camera_manager.cpp:313 libcamera v0.3.0+65-6ddd79b5
[component_container-1] [2:50:16.487658805] [5547]  INFO RPI pisp.cpp:695 libpisp version v1.0.6 b567f0455680 02-08-2024 (11:45:27)
[component_container-1] [2:50:16.489337316] [5547]  WARN CameraSensorProperties camera_sensor_properties.cpp:286 No static properties available for 'ov9281'
[component_container-1] [2:50:16.489352686] [5547]  WARN CameraSensorProperties camera_sensor_properties.cpp:288 Please consider updating the camera sensor properties database
[component_container-1] [2:50:16.492856338] [5547]  INFO RPI pisp.cpp:1154 Registered camera /base/axi/pcie@120000/rp1/i2c@88000/ov9281@60 to CFE device /dev/media3 and ISP device /dev/media0 using PiSP variant BCM2712_C0
[component_container-1] [2:50:16.493237933] [5532]  WARN V4L2 v4l2_pixelformat.cpp:344 Unsupported V4L2 pixel format RPBP
[component_container-1] [2:50:16.493628714] [5532]  INFO Camera camera.cpp:1183 configuring streams: (0) 640x480-YUYV
[component_container-1] [2:50:16.493768844] [5547]  INFO RPI pisp.cpp:1450 Sensor: /base/axi/pcie@120000/rp1/i2c@88000/ov9281@60 - Selected sensor format: 640x400-Y10_1X10 - Selected CFE format: 640x400-PC1M
[component_container-1] [INFO] [1722610081.462427016] [camera]: camera "/base/axi/pcie@120000/rp1/i2c@88000/ov9281@60" configured with 640x480-YUYV stream
[component_container-1] [WARN] [1722610081.463028612] [camera]: control NoiseReductionMode (10002) not handled
[component_container-1] [2:50:16.506082200] [5553]  WARN IPARPI ipa_base.cpp:1062 Could not set SHARPNESS - no sharpen algorithm
[component_container-1] [2:50:16.506142682] [5553]  WARN IPARPI ipa_base.cpp:1198 No HDR algorithm available
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/camera' in container '/camera_container'
[component_container-1] [INFO] [1722610081.527832303] [camera_container]: Load Library: /opt/ros/jazzy/lib/libimage_view_nodes.so
[component_container-1] [INFO] [1722610081.592056454] [camera_container]: Found class: rclcpp_components::NodeFactoryTemplate<image_view::DisparityViewNode>
[component_container-1] [INFO] [1722610081.592137454] [camera_container]: Found class: rclcpp_components::NodeFactoryTemplate<image_view::ExtractImagesNode>
[component_container-1] [INFO] [1722610081.592152065] [camera_container]: Found class: rclcpp_components::NodeFactoryTemplate<image_view::ImageSaverNode>
[component_container-1] [INFO] [1722610081.592163047] [camera_container]: Found class: rclcpp_components::NodeFactoryTemplate<image_view::ImageViewNode>
[component_container-1] [INFO] [1722610081.592174214] [camera_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<image_view::ImageViewNode>
[component_container-1] [INFO] [1722610081.599190055] [image_view_node]: Using transport "raw"
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/image_view_node' in container '/camera_container'
[component_container-1] QSocketNotifier: Can only be used with threads started with QThread
[component_container-1] [INFO] [1722610081.855659468] [camera]: using default calibration URL
[component_container-1] [INFO] [1722610081.855800914] [camera]: camera calibration URL: file:///home/lcv/.ros/camera_info/ov9281__base_axi_pcie_120000_rp1_i2c_88000_ov9281_60_640x480.yaml
[component_container-1] [ERROR] [1722610081.855907229] [camera_calibration_parsers]: Unable to open camera calibration file [/home/lcv/.ros/camera_info/ov9281__base_axi_pcie_120000_rp1_i2c_88000_ov9281_60_640x480.yaml]
[component_container-1] [WARN] [1722610081.855989174] [camera]: Camera calibration file /home/lcv/.ros/camera_info/ov9281__base_axi_pcie_120000_rp1_i2c_88000_ov9281_60_640x480.yaml not found
[component_container-1] terminate called after throwing an instance of 'cv::Exception'
[component_container-1]   what():  OpenCV(4.6.0) ./modules/imgproc/src/color.simd_helpers.hpp:92: error: (-2:Unspecified error) in function 'cv::impl::{anonymous}::CvtHelper<VScn, VDcn, VDepth, sizePolicy>::CvtHelper(cv::InputArray, cv::OutputArray, int) [with VScn = cv::impl::{anonymous}::Set<3, 4>; VDcn = cv::impl::{anonymous}::Set<3, 4>; VDepth = cv::impl::{anonymous}::Set<0, 2, 5>; cv::impl::{anonymous}::SizePolicy sizePolicy = cv::impl::<unnamed>::NONE; cv::InputArray = const cv::_InputArray&; cv::OutputArray = const cv::_OutputArray&]'
[component_container-1] > Invalid number of channels in input image:
[component_container-1] >     'VScn::contains(scn)'
[component_container-1] > where
[component_container-1] >     'scn' is 2
[component_container-1] 
[ERROR] [component_container-1]: process has died [pid 5532, exit code -6, cmd '/opt/ros/jazzy/lib/rclcpp_components/component_container --ros-args -r __node:=camera_container -r __ns:=/'].

I fixed the compressImageMsg function in the CameraNode.cpp file:

void
compressImageMsg(const sensor_msgs::msg::Image &source,
                 sensor_msgs::msg::CompressedImage &destination,
                 const std::vector<int> &params = std::vector<int>())
{
  std::shared_ptr<CameraNode> tracked_object;
  cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(source, tracked_object);

  destination.header = source.header;
  cv::Mat image;
  if (cv_ptr->encoding == enc::BGR8 || cv_ptr->encoding == enc::BGRA8)
  {
    cv::cvtColor(cv_ptr->image, image, cv::COLOR_BGR2GRAY);
  }
  else if (cv_ptr->encoding == enc::MONO8 || cv_ptr->encoding == enc::MONO16)
  {
    image = cv_ptr->image;
  }
  else {
    cv_bridge::CvImagePtr tempThis = std::make_shared<cv_bridge::CvImage>(*cv_ptr);
    cv_bridge::CvImagePtr temp;
    if (enc::hasAlpha(cv_ptr->encoding)) {
      temp = cv_bridge::cvtColor(tempThis, enc::BGRA8);
    }
    else {
      temp = cv_bridge::cvtColor(tempThis, enc::BGR8);
    }
    image = temp->image;
  } 

  destination.format = "jpg";
  cv::imencode(".jpg", image, destination.data, params);
}

but it had no effect

Where can I be wrong and how can I see the cv_ptr->encoding?

christianrauch commented 3 months ago

In the launch file, the CameraNode and ImageViewNode communicate via the raw topics. The function compressImageMsg should not be called unless you also manually subscribe to the compressed topic. Is this exception maybe thrown in the ImageViewNode?

Could you run the camera node individually (without the viewer) and subscribe to the raw and compressed topics manually to see if the exceptions originates indeed in the camera node? On the raw topic you can check encoding of the Image message to see the original image encoding.

If this still throws the exception, can you run the node with full debug information and paste the output here?

christianrauch commented 3 months ago

I can reproduce this exception in the ImageViewNode with the YUYV format.

Terminal A: start camera_node with YUYV:

ros2 run camera_ros camera_node --ros-args -p format:=YUYV

Terminal B: start image_view:

ros2 run image_view image_view --ros-args -r /image:=/camera/image_raw

causes:

> Invalid number of channels in input image:
>     'VScn::contains(scn)'
> where
>     'scn' is 2

The published image encoding is yuv422_yuy2:

$ ros2 topic echo /camera/image_raw --field encoding
yuv422_yuy2
---
christianrauch commented 3 months ago

See issue report: https://github.com/ros-perception/image_pipeline/issues/1021/

izeemeen commented 3 months ago

Thanks for answer, @christianrauch!

I check rpicam-hello --list-camera:

0 : ov9281 [1280x800 10-bit MONO] (/base/axi/pcie@120000/rp1/i2c@88000/ov9281@60) Modes: 'R8' : 640x400 [309.79 fps - (0, 0)/1280x800 crop] 1280x720 [171.79 fps - (0, 0)/1280x720 crop] 1280x800 [143.66 fps - (0, 0)/1280x800 crop] 'R10_CSI2P' : 640x400 [247.83 fps - (0, 0)/1280x800 crop] 1280x720 [137.42 fps - (0, 0)/1280x720 crop] 1280x800 [114.93 fps - (0, 0)/1280x800 crop]

Open camera.launch.py and change:

ComposableNode( package='camera_ros', plugin='camera::CameraNode', parameters=[{ "camera": 0, "width": 640, "height": 400, "format": 'XBGR8888, }], extra_arguments=[{'use_intra_process_comms': True}], ),

After this it works!

Or manually run:

ros2 run camera_ros camera_node --ros-args -p width:=640 -p height:=400 -p format:=XBGR8888

ros2 run image_view image_view --ros-args -r /image:=/camera/image_raw

christianrauch commented 3 months ago

Open camera.launch.py and change:

ComposableNode( package='camera_ros', plugin='camera::CameraNode', parameters=[{ "camera": 0, "width": 640, "height": 400, "format": 'XBGR8888, }], extra_arguments=[{'use_intra_process_comms': True}], ),

The launch file has a parameter format. You can directly provide the desired pixel format without needing to change the launch file:

ros2 launch camera_ros camera.launch.py format:=XBGR8888

In any case, this is a bug in the image viewer node. I am going to close this since the issue is unrelated to the camera node.