support color and depth/ir formats #2840
Conversation
SamerKhshiboun
force-pushed
the
support_opencv_and_ros_formats
branch
3 times, most recently
from
af89554
to
5455b9d
Compare
SamerKhshiboun
force-pushed
the
support_opencv_and_ros_formats
branch
3 times, most recently
from
583dc19
to
c0be4b9
Compare
| std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr> _info_publishers; | ||
| std::map<stream_index_pair, rclcpp::Publisher<realsense2_camera_msgs::msg::Metadata>::SharedPtr> _metadata_publishers; | ||
| std::map<stream_index_pair, rclcpp::Publisher<IMUInfo>::SharedPtr> _imu_info_publishers; | ||
| std::map<stream_index_pair, rclcpp::Publisher<Extrinsics>::SharedPtr> _extrinsics_publishers; | ||
| rclcpp::Publisher<realsense2_camera_msgs::msg::RGBD>::SharedPtr _rgbd_publisher; | ||
| std::map<stream_index_pair, cv::Mat> _images; | ||
| std::map<unsigned int, std::string> _encoding; | ||
| std::map<rs2_format, std::string> rs_format_to_ros_format; |
| rs_format_to_cv_format[RS2_FORMAT_BGR8] = CV_8UC3; | ||
| rs_format_to_cv_format[RS2_FORMAT_RGBA8] = CV_8UC4; | ||
| rs_format_to_cv_format[RS2_FORMAT_BGRA8] = CV_8UC4; | ||
| rs_format_to_cv_format[RS2_FORMAT_YUYV] = CV_8UC2; |
There was a problem hiding this comment.
I also need to add format of Y10BPack, will test this tomorrow with D415.
There was a problem hiding this comment.
After investigating, Y10BPack is not supported anymore.
| // Convert the CV::Mat into a ROS image message (1 copy is done here) | ||
| cv_bridge::CvImage(std_msgs::msg::Header(), _encoding.at(bpp), cv_matrix_image).toImageMsg(*img_msg_ptr); | ||
| cv_bridge::CvImage(std_msgs::msg::Header(), rs_format_to_ros_format[stream_format], cv_matrix_image).toImageMsg(*img_msg_ptr); |
There was a problem hiding this comment.
For unsupported format this will create a new entry, probably initialized with RS2_FORMAT_ANY. Is this OK?
There was a problem hiding this comment.
Unsupported formats will be stopped several steps before, and will switch the stream format to the default one (for example, for color, it will be RGB as default)
There was a problem hiding this comment.
I saw that you check above, but as I understand ROS_ERROR_STREAM does not throw (otherwise this won't work) and you don't return.
There was a problem hiding this comment.
Fixed functions to return status, and checked status after
(did this for two functions: when filling CV matrix image, and when filling ROS image msg)
| // we try to reduce image creation as much we can, so we check if the same image structure | ||
| // was already created before, and we fill this image next with the frame data | ||
| // image.create() should be called once per <stream>_<profile>_<format> | ||
| if (image.size() != cv::Size(width, height) || CV_MAKETYPE(image.depth(), image.channels()) != rs_format_to_cv_format[stream_format]) |
There was a problem hiding this comment.
same as above
There was a problem hiding this comment.
Fixed like above
| if (f.is<rs2::video_frame>()) | ||
| { | ||
| auto timage = f.as<rs2::video_frame>(); | ||
| width = timage.get_width(); | ||
| height = timage.get_height(); | ||
| bpp = timage.get_bytes_per_pixel(); | ||
| stream_format = timage.get_profile().format(); |
There was a problem hiding this comment.
Why will this produce different results than line 933?
There was a problem hiding this comment.
I think I tested this with some flow, will verify this again tomorrow.
There was a problem hiding this comment.
Fixed, and printed an error if check fail.
SamerKhshiboun
force-pushed
the
support_opencv_and_ros_formats
branch
2 times, most recently
from
9586de8
to
83ed9eb
Compare
SamerKhshiboun
force-pushed
the
support_opencv_and_ros_formats
branch
from
83ed9eb
to
6f40a4f
Compare
SamerKhshiboun
merged commit 40ac69f
into
IntelRealSense:ros2-development
Instead of supporting ROS Images of RGB, Y8 and Z16, now we support all ROS Images with these encodings: (depends on stream/profile chosen format):
RGB8, BGR8, RGBA8, BGRA8, Z16, Y8, Y16, RAW8, RAW10, RAW16, YUYV (YUV442)
Some tests on D455:
Color Y16:
ros2 launch realsense2_camera rs_launch.py rgb_camera.color_format:=Y16Color BGRA8:
ros2 launch realsense2_camera rs_launch.py rgb_camera.color_format:=BGRA8Color YUYV:
ros2 launch realsense2_camera rs_launch.py rgb_camera.color_format:=YUYV