订阅ROS Image和CameraInfo sensor.msgs格式

Question

我正在尝试订阅CameraInfo和格式为sensor_msgs.msg的图像，并使用其数据进行进一步的图像处理。以下是我开始使用的几行代码：

from sensor_msgs.msg import CameraInfo, Image
from cv_bridge import CvBridge, CvBridgeError
import cv2

if __name__ == '__main__':
    rospy.init_node('node_name')
    while ~rospy.is_shutdown():
        sub_cam_info = rospy.Subscriber('/camera/rgb/raw_camera_info', CameraInfo)
        sub_rgb = rospy.Subscriber('/camera/rgb/raw_image_color', Image)

从这里开始，我想从'sub_cam_info'和'sub_rgb'中提取标题和数据信息。像这样的东西：

camera_info_K = sub_cam_info.K
camera_info_dist_model = sub_cam_info.distortion_model
rgb_image = CvBridge().imgmsg_to_cv2(sub_rgb, encoding="rgb8")

然后使用此数据进行图像不失真： -

rgb_undist = cv2.undistort(rgb_img.data, camera_info_K, camera_info_dist_model)

主要是我在第二部分需要帮助，即从CameraInfo和Image消息中提取参数。但是，如果有人可以帮我整理整个代码的正文！我已经通过编写自己的CameraInfo和RGB消息，使用它们进行非失真并在ROS上发布，但现在我想从另一个ROS模块中获取它们并使用它们。

Answer 1

您是否通过使用sub_rgb检查是否获得了cv2.imshow()图像？

并打印出相机配置以检查是否成功获取？

您需要运行相机启动文件，然后启动node_name

执行此操作后，如果您收到错误，请告诉我。

我写了一个代码来获取ROS图像并将其转换为Mat（OpenCV格式）

Answer 2

对于像我这样的新手，我解决了这个问题：

import message_filters
import cv2
import rospy
from cv_bridge import CvBridge

def callback(rgb_msg, camera_info):
   rgb_image = CvBridge().imgmsg_to_cv2(rgb_msg, desired_encoding="rgb8")
   camera_info_K = np.array(camera_info.K).reshape([3, 3])
   camera_info_D = np.array(camera_info.D)
   rgb_undist = cv2.undistort(rgb_image, camera_info_K, camera_info_D)

if __name__ == '__main__':
   rospy.init_node('my_node', anonymous=True)
   image_sub = message_filters.Subscriber('/ardrone/front/image_raw', Image)
   info_sub = message_filters.Subscriber('/ardrone/front/camera_info', CameraInfo)
   ts = message_filters.ApproximateTimeSynchronizer([image_sub, info_sub], 10, 0.2)
   ts.registerCallback(callback)
   rospy.spin()

所以我的问题是如何订阅两个不同的主题并使用他们的数据进行进一步处理。 “message_filters.ApproximateTimeSynchronizer”根据接收到的每个消息的时间戳同步传入的消息，并且通过“ts.registerCallback（回调）”我能够将两个订阅的消息一起用于回调函数中的进一步处理。