sample_image_publisher.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 from __future__ import division
4 
5 import os.path as osp
6 
7 import cv2
8 import cv_bridge
9 import genpy
10 import numpy as np
11 import rospy
12 from sensor_msgs.msg import CameraInfo
13 from sensor_msgs.msg import Image
14 import yaml
15 
16 
17 class SampleImagePublisher(object):
18 
19  def __init__(self):
20  self.pub_img = rospy.Publisher(
21  '~output/image_raw', Image, queue_size=1)
22  self.pub_info = rospy.Publisher(
23  '~output/camera_info', CameraInfo, queue_size=1)
24 
25  img_file = osp.expanduser(rospy.get_param('~image_file'))
26  info_file = osp.expanduser(rospy.get_param('~camera_info_file'))
27  encoding = rospy.get_param('~encoding', 'passthrough')
28  frame_id = rospy.get_param('~frame_id', 'camera_rgb_optical_frame')
29  rate = rospy.get_param('~rate', 1.0)
30 
31  # read image from file
32  if img_file.endswith('.jpg') or img_file.endswith('.png'):
33  img = cv2.imread(img_file, cv2.IMREAD_UNCHANGED)
34  elif img_file.endswith('.npz'):
35  img = np.load(img_file)['arr_0']
36  else:
37  rospy.logerr('Unsupported file format: {}'.format(img_file))
38  bridge = cv_bridge.CvBridge()
39  self.imgmsg = bridge.cv2_to_imgmsg(img, encoding)
40  self.imgmsg.header.frame_id = frame_id
41 
42  # read camera_info from file
43  with open(info_file, 'r') as f:
44  info = yaml.load(f)
45  self.infomsg = CameraInfo()
46  genpy.message.fill_message_args(self.infomsg, info)
47 
48  rospy.Timer(rospy.Duration(1 / rate), self._cb)
49 
50  def _cb(self, event):
51  self.imgmsg.header.stamp = event.current_real
52  self.infomsg.header.stamp = event.current_real
53  self.pub_img.publish(self.imgmsg)
54  self.pub_info.publish(self.infomsg)
55 
56 
57 if __name__ == '__main__':
58  rospy.init_node('sample_image_publisher')
60  rospy.spin()


jsk_recognition_utils
Author(s):
autogenerated on Fri Dec 6 2019 04:02:51