订阅ROS图像和CameraInfo sensor.msgs格式

时间:2019-04-21 08:22:15

标签: image python-2.7 publish-subscribe ros

我试图订阅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模块中获取它们并使用它们。

2 个答案:

答案 0 :(得分:0)

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

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

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

在执行此操作后,如果出现错误,请通知我。

我编写了代码以获取ROS图像并将其转换为Mat(OpenCV格式)

答案 1 :(得分:0)

好吧,像我这样的新手,我这样解决了:

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.roximateTimeSynchronizer”根据时间戳与接收到的每个消息以及“ ts.registerCallback(callback)”同步传入的消息,从而能够将两个订阅的消息一起用于回调函数中的进一步处理。