Подписаться на ROS изображение и формат 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

Хорошо для новичков, как я, я решил это так:

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 (callback)» я смог использовать оба подписанных сообщения вместе для дальнейшей обработки в функции обратного вызова.

Answer 2

Вы проверяли, получаете ли вы изображение sub_rgb, используя cv2.imshow()?

И распечатал конфигурацию камеры, чтобы проверить, была ли она получена успешно?

Вам нужно запустить файл запуска камеры, а затем запустить node_name

После этого, если вы получили ошибку, дайте мне знать.

Я написал код, чтобы получить изображение ROS и преобразовать его в Mat (формат OpenCV)