从gazebo启动的world发布的topic中，获取图像数据并保存到本地

目录

1.  首先， xxx.world 里有图像传感器。

2. gazebo启动场景

3. 查看image的topic是否存在

4. 查看 /camera/image_raw 详细信息

5. 使用 ROS2的客户端库(python) rclpy 获取图像数据， 并每隔0.5秒，保存一张到本地.

1.  首先， xxx.world 里有图像传感器。

  <sensor name='camera' type='depth'>
	<visualize>0</visualize>
	<update_rate>15</update_rate>
	<camera>
	  <image>
		<format>B8G8R8</format>
		<width>640</width>
		<height>480</height>
	  </image>
	  <clip>
		<near>0.28</near>
		<far>10</far>
	  </clip>
	  <horizontal_fov>1.047</horizontal_fov>
	</camera>
	<plugin name='camera_controller' filename='libgazebo_ros_camera.so'>
	  <frame_name>camera_link_optical</frame_name>
	  <min_depth>0.28</min_depth>
	  <max_depth>6.0</max_depth>
	</plugin>
	<pose>0.21 0 0.1 0 -0 0</pose>
  </sensor>
</link>

2. gazebo启动场景

$ gazebo xxx.world

3. 查看image的topic是否存在

$ ros2 topic list -t
# result:/camera/image_raw [sensor_msgs/msg/Image] 存在 

运行结果：

/camera/camera_info [sensor_msgs/msg/CameraInfo]
/camera/depth/camera_info [sensor_msgs/msg/CameraInfo]
/camera/depth/image_raw [sensor_msgs/msg/Image]
/camera/image_raw [sensor_msgs/msg/Image]
/camera/points [sensor_msgs/msg/PointCloud2]
...

4. 查看 /camera/image_raw 详细信息

$ ros2 topic echo /camera/image_raw
# result: height: 480 width: 640 encoding: bgr8

运行结果：  

header:
  stamp:
    sec: 2077
    nanosec: 790000000
  frame_id: camera_link_optical
height: 480
width: 640
encoding: bgr8
is_bigendian: 0
step: 1920
data:
- 178
...

5. 使用 ROS2的客户端库(python) rclpy 获取图像数据， 并每隔0.5秒，保存一张到本地.

import os
import sys
import time
# ROS2的客户端库(python) rclpy
import rclpy
from rclpy.qos import qos_profile_sensor_data
from rclpy.node import Node
# image-----below----
# topic sensor_msgs/msg/Image to cv2
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

# import snoop


def cv2_imshow(win_name: str, img_mat) -> None:
    """
    显示接受到的图像数据
    :param win_name:
    :param img_mat:
    :return:
    """
    cv2.imshow(win_name, img_mat)
    # cv2.waitKey(0)  # delay – Delay in milliseconds. 0 is the special value that means “forever”.
    if cv2.waitKey(1) & 0xFF == ord('q'):  # 当你按下q键时，显示图片的窗口被关掉，并打印出I'm done，结束程序
        print("I'm done")
        sys.exit(0)


class ImageNode(Node):
    # Node constructor
    def __init__(self) -> None:
        super().__init__("image_node")

        self.print_count = 1
        self.img_count = 1
        self.cv_bridge = CvBridge()

        # get from topic /camera/image_raw [sensor_msgs/msg/Image]
        self._image_sub = self.create_subscription(
            Image,                   # topic_type : sensor_msgs/msg/Image
            "/camera/image_raw",     # topic_name :  /camera/image_raw
            self.image_cb,           # image_listener_callback
            qos_profile_sensor_data  # “queue size” is 10. Queue size is a required QoS
        )

    # @snoop
    def image_cb(self, msg: Image) -> None:
        """
        image listener callback
        :param msg:
        :return:
        """
        if self.print_count:
            print("msg.header :  {}".format(
                msg.header))  # msg.header: std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=23495, nanosec=888000000), frame_id='camera_link_optical')
            print("msg.encoding :  {}".format(msg.encoding))  # msg.encoding: bgr8
            # print("msg.data : \n {}".format(msg.data))
            print("msg.height :  {}".format(msg.height))  # msg.height: 480
            print("msg.width : {}".format(msg.width))  # msg.width: 640
            self.print_count = 0

        # record start time
        # start_time = time.perf_counter()

        # convert to cv image & predict
        cv_image = self.cv_bridge.imgmsg_to_cv2(msg, "bgr8")
        save_dir = "save"
        img_path = os.path.join(save_dir, str(self.img_count).zfill(4) + ".jpg")
        cv2.imwrite(filename=img_path, img=cv_image)
        print(img_path)
        self.img_count += 1
        time.sleep(0.5)  # 0.5s

        # end_time = time.perf_counter()
        # print(f"{end_time - start_time = }")

        # display
        # cv2_imshow("image_raw", cv_image)


def main(args=None):
    rclpy.init(args=args)
    node = ImageNode()
    rclpy.spin(node)
    rclpy.shutdown()
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()