ros节点模拟摄像头发布图片数据。

最新推荐文章于 2024-08-14 22:35:10 发布

qq_278667286

最新推荐文章于 2024-08-14 22:35:10 发布

阅读量817

点赞数 1

分类专栏： ros python 文章标签： python ros

本文链接：https://blog.csdn.net/qq_38288618/article/details/122258574

版权

ros 同时被 2 个专栏收录

64 篇文章 18 订阅

订阅专栏

python

26 篇文章 0 订阅

订阅专栏

ros节点模拟摄像头发布图片数据

# coding=utf-8
#!/usr/bin/env python
#coding:utf-8

import rospy
import sys
sys.path.append('.')
import cv2
import os
import math
import numpy as np
#from test_py.msg import Image
from sensor_msgs.msg import Image
from sensor_msgs.msg import CameraInfo
from cv_bridge import CvBridge, CvBridgeError
def build_camera_info( attributes):  # pylint: disable=no-self-use

        camera_info = CameraInfo()
        # store info without header
        camera_info.header = None
        camera_info.width = int(attributes['width'])
        camera_info.height = int(attributes['height'])
        camera_info.distortion_model = 'plumb_bob'
        cx = camera_info.width / 2.0
        cy = camera_info.height / 2.0
        fx = camera_info.width / (
             2.0 *math.tan(float(attributes['fov']) * math.pi / 360.0))
        #像高 = EFL*tan (半FOV)
        #view_angle = 45*math.pi/180
        #f_x = (width/2)/math.tan(view_angle/2);
        fy = fx
        camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
        camera_info.D = [0, 0, 0, 0, 0]
        camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
        camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
        return camera_info 
def pubImage():
    rospy.init_node('pubImage',anonymous = True)
    pub = rospy.Publisher('/kinect2/hd/image_color', Image, queue_size = 1)
    pub_info = rospy.Publisher('/kinect2/hd/camera_info', CameraInfo, queue_size=1)
    rate = rospy.Rate(1)
    bridge = CvBridge()
    gt_imdb = []
    #path是我存放图片的文件夹
    path = "./img/"
    for item in os.listdir(path):
        gt_imdb.append(os.path.join(path,item))
    while not rospy.is_shutdown():
        for imagepath in gt_imdb:
            image = cv2.imread(imagepath)
            image = cv2.resize(image,(2048,2048))
            imagess = cv2.resize(image,(800,800))
            attributes={}
            attributes['width']=2048
            attributes['height']=2048
            attributes['fov']=45
            ci=build_camera_info(attributes)
            imageMsg=bridge.cv2_to_imgmsg(image,"bgr8")
            imageMsg.header.frame_id="kinect2_rgb_optical_frame"
            ci.header=imageMsg.header
            cv2.imshow("img",imagess)

            while 1:
                if cv2.waitKey(1) == ord('z'):
                    break
                if rospy.is_shutdown():
                    break
                pub.publish(imageMsg)
                pub_info.publish(ci)
                rate.sleep()
            if rospy.is_shutdown():
                break
            
            #cv2.waitKey(0)


if __name__ == '__main__':
    try:
        pubImage()
    except rospy.ROSInterruptException:
        pass