ROS2之OpenCV怎么理解一段代码

zhangrelay

已于 2022-06-16 16:39:16 修改

阅读量1.7k

点赞数 5

文章标签： ROS2 OpenCV

于 2022-06-16 16:38:49 首次发布

本文链接：https://blog.csdn.net/ZhangRelay/article/details/125317070

版权

看懂代码才能掌握，理解代码之后开启应用改进等。

最简单的一个问题，彩色图变灰度图。

答案如下：

im = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY )

为什么？？？

那么回到：

ROS2之OpenCV的微笑入门资料篇_zhangrelay的博客-CSDN博客

其中，有一篇博客：

ROS2之OpenCV基础代码对比foxy~galactic~humble_zhangrelay的博客-CSDN博客

里面代码如下：

# Basic ROS 2 program to publish real-time streaming 
# video from your built-in webcam
# Author:
# - Addison Sears-Collins
# - https://automaticaddison.com
 
# Import the necessary libraries
import rclpy # Python Client Library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
from sensor_msgs.msg import Image # Image is the message type
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
import cv2 # OpenCV library

class ImagePublisher(Node):
  """
  Create an ImagePublisher class, which is a subclass of the Node class.
  """
  def __init__(self):
    """
    Class constructor to set up the node
    """
    # Initiate the Node class's constructor and give it a name
    super().__init__('image_publisher')
     
    # Create the publisher. This publisher will publish an Image
    # to the video_frames topic. The queue size is 10 messages.
    self.publisher_ = self.create_publisher(Image, 'video_frames', 10)
     
    # We will publish a message every 0.1 seconds
    timer_period = 0.1  # seconds
     
    # Create the timer
    self.timer = self.create_timer(timer_period, self.timer_callback)
		
    # Create a VideoCapture object
    # The argument '0' gets the default webcam.
    self.cap = cv2.VideoCapture(0)
		
    # Used to convert between ROS and OpenCV images
    self.br = CvBridge()
  
  def timer_callback(self):
    """
    Callback function.
    This function gets called every 0.1 seconds.
    """
    # Capture frame-by-frame
    # This method returns True/False as well
    # as the video frame.
    ret, frame = self.cap.read()
         
    if ret == True:
      # Publish the image.
      # The 'cv2_to_imgmsg' method converts an OpenCV
      # image to a ROS 2 image message
      self.publisher_.publish(self.br.cv2_to_imgmsg(frame))

    # Display the message on the console
    self.get_logger().info('Publishing video frame')
 
def main(args=None):
 
  # Initialize the rclpy library
  rclpy.init(args=args)
 
  # Create the node
  image_publisher = ImagePublisher()
 
  # Spin the node so the callback function is called.
  rclpy.spin(image_publisher)
 
  # Destroy the node explicitly
  # (optional - otherwise it will be done automatically
  # when the garbage collector destroys the node object)
  image_publisher.destroy_node()
 
  # Shutdown the ROS client library for Python
  rclpy.shutdown()
 
if __name__ == '__main__':
  main()

虽然有注释，但看起来还是晕晕的，尤其是如果遇到报错，或者要改动实现一些其他功能无从下手。

此时，需要参考如下：

# Software License Agreement (BSD License)
#
# Copyright (c) 2011, Willow Garage, Inc.
# Copyright (c) 2016, Tal Regev.
# Copyright (c) 2018 Intel Corporation.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
#  * Redistributions of source code must retain the above copyright
#    notice, this list of conditions and the following disclaimer.
#  * Redistributions in binary form must reproduce the above
#    copyright notice, this list of conditions and the following
#    disclaimer in the documentation and/or other materials provided
#    with the distribution.
#  * Neither the name of Willow Garage, Inc. nor the names of its
#    contributors may be used to endorse or promote products derived
#    from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
####################################################################

import sys

import sensor_msgs.msg


class CvBridgeError(TypeError):
    """This is the error raised by :class:`cv_bridge.CvBridge` methods when they fail."""

    pass


class CvBridge(object):
    """
    The CvBridge is an object that converts between OpenCV Images and ROS Image messages.

       .. doctest::
           :options: -ELLIPSIS, +NORMALIZE_WHITESPACE

           >>> import cv2
           >>> import numpy as np
           >>> from cv_bridge import CvBridge
           >>> br = CvBridge()
           >>> dtype, n_channels = br.encoding_as_cvtype2('8UC3')
           >>> im = np.ndarray(shape=(480, 640, n_channels), dtype=dtype)
           >>> msg = br.cv2_to_imgmsg(im)  # Convert the image to a message
           >>> im2 = br.imgmsg_to_cv2(msg) # Convert the message to a new image
           >>> # Convert the image to a compress message
           >>> cmprsmsg = br.cv2_to_compressed_imgmsg(im)
           >>> # Convert the compress message to a new image
           >>> im22 = br.compressed_imgmsg_to_cv2(msg)
           >>> cv2.imwrite("this_was_a_message_briefly.png", im2)

    """

    def __init__(self):
        import cv2
        self.cvtype_to_name = {}
        self.cvdepth_to_numpy_depth = {cv2.CV_8U: 'uint8', cv2.CV_8S: 'int8',
                                       cv2.CV_16U: 'uint16', cv2.CV_16S: 'int16',
                                       cv2.CV_32S: 'int32', cv2.CV_32F: 'float32',
                                       cv2.CV_64F: 'float64'}

        for t in ['8U', '8S', '16U', '16S', '32S', '32F', '64F']:
            for c in [1, 2, 3, 4]:
                nm = '%sC%d' % (t, c)
                self.cvtype_to_name[getattr(cv2, 'CV_%s' % nm)] = nm

        self.numpy_type_to_cvtype = {'uint8': '8U', 'int8': '8S', 'uint16': '16U',
                                     'int16': '16S', 'int32': '32S', 'float32': '32F',
                                     'float64': '64F'}
        self.numpy_type_to_cvtype.update(dict((v, k) for (k, v) in self.numpy_type_to_cvtype.items()))

    def dtype_with_channels_to_cvtype2(self, dtype, n_channels):
        return '%sC%d' % (self.numpy_type_to_cvtype[dtype.name], n_channels)

    def cvtype2_to_dtype_with_channels(self, cvtype):
        from cv_bridge.boost.cv_bridge_boost import CV_MAT_CNWrap, CV_MAT_DEPTHWrap
        return self.cvdepth_to_numpy_depth[CV_MAT_DEPTHWrap(cvtype)], CV_MAT_CNWrap(cvtype)

    def encoding_to_cvtype2(self, encoding):
        from cv_bridge.boost.cv_bridge_boost import getCvType

        try:
            return getCvType(encoding)
        except RuntimeError as e:
            raise CvBridgeError(e)

    def encoding_to_dtype_with_channels(self, encoding):
        return self.cvtype2_to_dtype_with_channels(self.encoding_to_cvtype2(encoding))

    def compressed_imgmsg_to_cv2(self, cmprs_img_msg, desired_encoding='passthrough'):
        """
        Convert a sensor_msgs::CompressedImage message to an OpenCV :cpp:type:`cv::Mat`.

        :param cmprs_img_msg:   A :cpp:type:`sensor_msgs::CompressedImage` message
        :param desired_encoding:  The encoding of the image data, one of the following strings:

           * ``"passthrough"``
           * one of the standard strings in sensor_msgs/image_encodings.h

        :rtype: :cpp:type:`cv::Mat`
        :raises CvBridgeError: when conversion is not possible.

        If desired_encoding is ``"passthrough"``, then the returned image has the same format
        as img_msg. Otherwise desired_encoding must be one of the standard image encodings

        This function returns an OpenCV :cpp:type:`cv::Mat` message on success,
        or raises :exc:`cv_bridge.CvBridgeError` on failure.

        If the image only has one channel, the shape has size 2 (width and height)
        """
        import cv2
        import numpy as np

        str_msg = cmprs_img_msg.data
        buf = np.ndarray(shape=(1, len(str_msg)),
                         dtype=np.uint8, buffer=cmprs_img_msg.data)
        im = cv2.imdecode(buf, cv2.IMREAD_ANYCOLOR)

        if desired_encoding == 'passthrough':
            return im

        from cv_bridge.boost.cv_bridge_boost import cvtColor2

        try:
            res = cvtColor2(im, 'bgr8', desired_encoding)
        except RuntimeError as e:
            raise CvBridgeError(e)

        return res

    def imgmsg_to_cv2(self, img_msg, desired_encoding='passthrough'):
        """
        Convert a sensor_msgs::Image message to an OpenCV :cpp:type:`cv::Mat`.

        :param img_msg:   A :cpp:type:`sensor_msgs::Image` message
        :param desired_encoding:  The encoding of the image data, one of the following strings:

           * ``"passthrough"``
           * one of the standard strings in sensor_msgs/image_encodings.h

        :rtype: :cpp:type:`cv::Mat`
        :raises CvBridgeError: when conversion is not possible.

        If desired_encoding is ``"passthrough"``, then the returned image has the same format
        as img_msg. Otherwise desired_encoding must be one of the standard image encodings

        This function returns an OpenCV :cpp:type:`cv::Mat` message on success,
        or raises :exc:`cv_bridge.CvBridgeError` on failure.

        If the image only has one channel, the shape has size 2 (width and height)
        """
        import numpy as np
        dtype, n_channels = self.encoding_to_dtype_with_channels(img_msg.encoding)
        dtype = np.dtype(dtype)
        dtype = dtype.newbyteorder('>' if img_msg.is_bigendian else '<')

        img_buf = np.asarray(img_msg.data, dtype=dtype) if isinstance(img_msg.data, list) else img_msg.data

        if n_channels == 1:
            im = np.ndarray(shape=(img_msg.height, int(img_msg.step/dtype.itemsize)),
                            dtype=dtype, buffer=img_buf)
            im = np.ascontiguousarray(im[:img_msg.height, :img_msg.width])
        else:
            im = np.ndarray(shape=(img_msg.height, int(img_msg.step/dtype.itemsize/n_channels), n_channels),
                            dtype=dtype, buffer=img_buf)
            im = np.ascontiguousarray(im[:img_msg.height, :img_msg.width, :])

        # If the byte order is different between the message and the system.
        if img_msg.is_bigendian == (sys.byteorder == 'little'):
            im = im.byteswap().newbyteorder()

        if desired_encoding == 'passthrough':
            return im

        from cv_bridge.boost.cv_bridge_boost import cvtColor2

        try:
            res = cvtColor2(im, img_msg.encoding, desired_encoding)
        except RuntimeError as e:
            raise CvBridgeError(e)

        return res

    def cv2_to_compressed_imgmsg(self, cvim, dst_format='jpg'):
        """
        Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::CompressedImage message.

        :param cvim:      An OpenCV :cpp:type:`cv::Mat`
        :param dst_format:  The format of the image data, one of the following strings:

        http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html
        http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat
        * imread(const string& filename, int flags)
           * bmp, dib
           * jpeg, jpg, jpe
           * jp2
           * png
           * pbm, pgm, ppm
           * sr, ras
           * tiff, tif

        :rtype:           A sensor_msgs.msg.CompressedImage message
        :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``format``


        This function returns a sensor_msgs::Image message on success,
        or raises :exc:`cv_bridge.CvBridgeError` on failure.
        """
        import cv2
        import numpy as np
        if not isinstance(cvim, (np.ndarray, np.generic)):
            raise TypeError('Your input type is not a numpy array')
        cmprs_img_msg = sensor_msgs.msg.CompressedImage()
        cmprs_img_msg.format = dst_format
        ext_format = '.' + dst_format
        try:
            cmprs_img_msg.data.frombytes(np.array(cv2.imencode(ext_format, cvim)[1]).tobytes())
        except RuntimeError as e:
            raise CvBridgeError(e)

        return cmprs_img_msg

    def cv2_to_imgmsg(self, cvim, encoding='passthrough', header = None):
        """
        Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message.

        :param cvim:      An OpenCV :cpp:type:`cv::Mat`
        :param encoding:  The encoding of the image data, one of the following strings:

           * ``"passthrough"``
           * one of the standard strings in sensor_msgs/image_encodings.h
        :param header:    A std_msgs.msg.Header message

        :rtype:           A sensor_msgs.msg.Image message
        :raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding``

        If encoding is ``"passthrough"``, then the message has the same encoding as the image's
        OpenCV type. Otherwise desired_encoding must be one of the standard image encodings

        This function returns a sensor_msgs::Image message on success,
        or raises :exc:`cv_bridge.CvBridgeError` on failure.
        """
        import numpy as np
        if not isinstance(cvim, (np.ndarray, np.generic)):
            raise TypeError('Your input type is not a numpy array')
        img_msg = sensor_msgs.msg.Image()
        img_msg.height = cvim.shape[0]
        img_msg.width = cvim.shape[1]
        if header is not None:
            img_msg.header = header
        if len(cvim.shape) < 3:
            cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, 1)
        else:
            cv_type = self.dtype_with_channels_to_cvtype2(cvim.dtype, cvim.shape[2])
        if encoding == 'passthrough':
            img_msg.encoding = cv_type
        else:
            img_msg.encoding = encoding
            # Verify that the supplied encoding is compatible with the type of the OpenCV image
            if self.cvtype_to_name[self.encoding_to_cvtype2(encoding)] != cv_type:
                raise CvBridgeError('encoding specified as %s, but image has incompatible type %s'
                                    % (encoding, cv_type))
        if cvim.dtype.byteorder == '>':
            img_msg.is_bigendian = True
        img_msg.data.frombytes(cvim.tobytes())
        img_msg.step = len(img_msg.data) // img_msg.height

        return img_msg

在官方教程没有完整度不高的情况下，阅读源码中注释，能更好的理解代码。

// Check for the most common encodings first
if (encoding == enc::BGR8) {return CV_8UC3;}
if (encoding == enc::MONO8) {return CV_8UC1;}
if (encoding == enc::RGB8) {return CV_8UC3;}
if (encoding == enc::MONO16) {return CV_16UC1;}
if (encoding == enc::BGR16) {return CV_16UC3;}
if (encoding == enc::RGB16) {return CV_16UC3;}
if (encoding == enc::BGRA8) {return CV_8UC4;}
if (encoding == enc::RGBA8) {return CV_8UC4;}
if (encoding == enc::BGRA16) {return CV_16UC4;}
if (encoding == enc::RGBA16) {return CV_16UC4;}

如果完全按教程中走，会出现各种报错^_^

OpenCV-Python 是一个 Python 绑定库，旨在解决计算机视觉问题。 cv2.cvtColor() 方法用于将图像从一种颜色空间转换为另一种颜色空间。 OpenCV 中有超过 150 种颜色空间转换方法可用。我们将在下面使用一些色彩空间转换代码。

语法：cv2.cvtColor(src, code[, dst[, dstCn]])

参数：
src：要改变颜色空间的图像。
code：颜色空间转换代码。
dst：与src图像大小和深度相同的输出图像。它是一个可选参数。
dstCn：目标图像的通道数。如果参数为 0，则通道数自动从 src 和 code 导出。它是一个可选参数。

return：它返回一个图像。

案例一：

# Python program to explain cv2.cvtColor() method 
   
# importing cv2 
import cv2 
   
# path 
path = r'C:\Users\Administrator\Desktop\geeks.png'
   
# Reading an image in default mode
src = cv2.imread(path)
   
# Window name in which image is displayed
window_name = 'Image'
  
# Using cv2.cvtColor() method
# Using cv2.COLOR_BGR2GRAY color space
# conversion code
image = cv2.cvtColor(src, cv2.COLOR_BGR2GRAY )
  
# Displaying the image 
cv2.imshow(window_name, image)

案例二：

# Python program to explain cv2.cvtColor() method 
   
# importing cv2 
import cv2 
   
# path 
path = r'C:\Users\Administrator\Desktop\geeks.png'
   
# Reading an image in default mode
src = cv2.imread(path)
   
# Window name in which image is displayed
window_name = 'Image'
  
# Using cv2.cvtColor() method
# Using cv2.COLOR_BGR2HSV color space
# conversion code
image = cv2.cvtColor(src, cv2.COLOR_BGR2HSV )
  
# Displaying the image 
cv2.imshow(window_name, image)

案例三：

移植到ROS2中？

答案已在文首。

附录：

颜色空间表示不一致

ImageView.callback_image() could not convert image from '8UC3' to 'rgb8' ([8UC3] is not a color format. but [rgb8] is. The conversion does not make sense)
ImageView.callback_image() could not convert image from '8UC3' to 'rgb8' ([8UC3] is not a color format. but [rgb8] is. The conversion does not make sense)
ImageView.callback_image() could not convert image from '8UC3' to 'rgb8' ([8UC3] is not a color format. but [rgb8] is. The conversion does not make sense)
ImageView.callback_image() could not convert image from '8UC3' to 'rgb8' ([8UC3] is not a color format. but [rgb8] is. The conversion does not make sense)
ImageView.callback_image() could not convert image from '8UC3' to 'rgb8' ([8UC3] is not a color format. but [rgb8] is. The conversion does not make sense)

库不全

C:\ros_ws\pythondemo>python cvpub2.py
Traceback (most recent call last):
  File "cvpub2.py", line 162, in <module>
    main()
  File "cvpub2.py", line 140, in main
    rclpy.spin(image_publisher)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\__init__.py", line 191, in spin
    executor.spin_once()
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 687, in spin_once
    raise handler.exception()
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\task.py", line 239, in __call__
    self._handler.send(None)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 407, in handler
    await call_coroutine(entity, arg)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 321, in _execute_timer
    await await_or_execute(tmr.callback)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 118, in await_or_execute
    return callback(*args)
  File "cvpub2.py", line 98, in timer_callback
    dtype, n_channels = self.br.encoding_as_cvtype2('8UC3')
AttributeError: 'CvBridge' object has no attribute 'encoding_as_cvtype2'
[ WARN:0] global C:\Users\appveyor\AppData\Local\Temp\1\pip-req-build-9d_dfo3_\opencv\modules\videoio\src\cap_msmf.cpp (435) `anonymous-namespace'::SourceReaderCB::~SourceReaderCB terminating async callback

C:\ros_ws\pythondemo>python cvpub2.py
Traceback (most recent call last):
  File "cvpub2.py", line 162, in <module>
    main()
  File "cvpub2.py", line 140, in main
    rclpy.spin(image_publisher)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\__init__.py", line 191, in spin
    executor.spin_once()
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 687, in spin_once
    raise handler.exception()
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\task.py", line 239, in __call__
    self._handler.send(None)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 407, in handler
    await call_coroutine(entity, arg)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 321, in _execute_timer
    await await_or_execute(tmr.callback)
  File "c:\opt\ros\foxy\x64\lib\site-packages\rclpy\executors.py", line 118, in await_or_execute
    return callback(*args)
  File "cvpub2.py", line 98, in timer_callback
    dtype, n_channels = self.br.encoding_to_cvtype2('8UC3')
  File "c:\opt\ros\foxy\x64\lib\site-packages\cv_bridge\core.py", line 96, in encoding_to_cvtype2
    from cv_bridge.boost.cv_bridge_boost import getCvType
ModuleNotFoundError: No module named 'cv_bridge.boost.cv_bridge_boost'
[ WARN:0] global C:\Users\appveyor\AppData\Local\Temp\1\pip-req-build-9d_dfo3_\opencv\modules\videoio\src\cap_msmf.cpp (435) `anonymous-namespace'::SourceReaderCB::~SourceReaderCB terminating async callback

C:\ros_ws\pythondemo>

zhangrelay

关注

5
点赞
踩
3

收藏

觉得还不错? 一键收藏
打赏
0
评论
ROS2之OpenCV怎么理解一段代码

看懂代码才能掌握，理解代码之后开启应用改进等。最简单的一个问题，彩色图变灰度图。答案如下：im = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY )为什么？？？那么回到：ROS2之OpenCV的微笑入门资料篇_zhangrelay的博客-CSDN博客其中，有一篇博客：ROS2之OpenCV基础代码对比foxy~galactic~humble_zhangrelay的博客-CSDN博客里面代码如下：# Basic ROS 2 .
复制链接

扫一扫