def write(self, data):
np_arr = np.frombuffer(data, dtype=np.uint8)
img = np_arr.reshape((RES[1], RES[0], 3))
# Publish compressed image
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', img)[1]).tobytes()
self.pub_img_compressed.publish(msg)
# Publish raw image
if self.publish_raw:
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.width = RES[0]
msg.height = RES[1]
msg.encoding = "bgr8"
msg.step = len(data) // RES[1]
msg.data = data
self.pub_img.publish(msg)
def publish_video(self, vid_type, vid_data, seq=None):
# convert data
if vid_type == 1 or vid_type == 2: # /dev/video1 data
vid_data = conversionlib.convert(vid_data, VID1_WIDTH, VID1_HEIGHT)
# gather data/info
mesginfo = CameraInfo()
if vid_type == 0: # video0
mesgdata = CompressedImage()
mesgdata.format = "jpeg"
mesginfo.width = VID0_WIDTH
mesginfo.height = VID0_HEIGHT
channel = 0
elif vid_type == 1: # video1
mesgdata = Image()
mesgdata.width = VID1_WIDTH
mesgdata.height = VID1_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID1_WIDTH
mesginfo.height = VID1_HEIGHT
channel = 1
elif vid_type == 2: # video2
mesgdata = Image()
mesgdata.width = VID1_WIDTH
mesgdata.height = VID1_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID1_WIDTH
mesginfo.height = VID1_HEIGHT
channel = 2
elif vid_type == 'changed_video1': # e.g. resize, delay
mesgdata = Image()
mesgdata.width = VID1_WIDTH
mesgdata.height = VID1_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID1_WIDTH
mesginfo.height = VID1_HEIGHT
channel = 2
elif vid_type == 'elf_video':
mesgdata = Image()
mesgdata.width = VID0_WIDTH
mesgdata.height = VID0_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID0_WIDTH
mesginfo.height = VID0_HEIGHT
mesginfo.header.seq = seq
channel = 0
else:
raise Exception("bad vid_type: (vid_type = %s)" % vid_type)
mesgdata.header.stamp = rospy.Time.now()
mesgdata.data = vid_data
# note: distortion model not used by PTAM(M)
#mesginfo.distortion_model = 'plumb_bob'
#mesginfo.D = [-0.33584000000000003, 0.12129000000000001, -0.0011500000000000002, 0.00039000000000000005, 0.0]
#mesginfo.K = [431.90402, 0.0, 313.01760999999999, 0.0, 432.04253999999997, 246.35373999999999, 0.0, 0.0, 1.0]
#mesginfo.R = [ 0.99995000000000012, 0.0089900000000000015, -0.0040000000000000001, \
# -0.0089700000000000005, 0.99996000000000007, 0.0029100000000000003, \
# 0.0040300000000000006, -0.0028700000000000002, 0.99999000000000005]
#mesginfo.P = [395.29849999999999, 0.0, 315.97555, 0.0, 0.0, 395.29849999999999, 238.45780999999999, 0.0, 0.0, 0.0, 1.0, 0.0]
#mesginfo.header.frame_id = 'mono_optical_frame'
mesginfo.header.stamp = mesgdata.header.stamp
# publish data/info
self.pub_vid_data[channel].publish(mesgdata)
self.pub_vid_info[channel].publish(mesginfo)
def publish_video(self, vid_type, vid_data, seq=None):
# convert data
if vid_type == 1 or vid_type == 2: # /dev/video1 data
vid_data = conversionlib.convert(vid_data, VID1_WIDTH, VID1_HEIGHT)
# gather data/info
mesginfo = CameraInfo()
if vid_type == 0: # video0
mesgdata = CompressedImage()
mesgdata.format = "jpeg"
mesginfo.width = VID0_WIDTH
mesginfo.height = VID0_HEIGHT
channel = 0
elif vid_type == 1: # video1
mesgdata = Image()
mesgdata.width = VID1_WIDTH
mesgdata.height = VID1_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID1_WIDTH
mesginfo.height = VID1_HEIGHT
channel = 1
elif vid_type == 2: # video2
mesgdata = Image()
mesgdata.width = VID1_WIDTH
mesgdata.height = VID1_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID1_WIDTH
mesginfo.height = VID1_HEIGHT
channel = 2
elif vid_type == "changed_video1": # e.g. resize, delay
mesgdata = Image()
mesgdata.width = VID1_WIDTH
mesgdata.height = VID1_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID1_WIDTH
mesginfo.height = VID1_HEIGHT
channel = 2
elif vid_type == "elf_video":
mesgdata = Image()
mesgdata.width = VID0_WIDTH
mesgdata.height = VID0_HEIGHT
mesgdata.encoding = "rgb8"
mesginfo.width = VID0_WIDTH
mesginfo.height = VID0_HEIGHT
mesginfo.header.seq = seq
channel = 0
else:
raise Exception("bad vid_type: (vid_type = %s)" % vid_type)
mesgdata.header.stamp = rospy.Time.now()
mesgdata.data = vid_data
# note: distortion model not used by PTAM(M)
# mesginfo.distortion_model = 'plumb_bob'
# mesginfo.D = [-0.33584000000000003, 0.12129000000000001, -0.0011500000000000002, 0.00039000000000000005, 0.0]
# mesginfo.K = [431.90402, 0.0, 313.01760999999999, 0.0, 432.04253999999997, 246.35373999999999, 0.0, 0.0, 1.0]
# mesginfo.R = [ 0.99995000000000012, 0.0089900000000000015, -0.0040000000000000001, \
# -0.0089700000000000005, 0.99996000000000007, 0.0029100000000000003, \
# 0.0040300000000000006, -0.0028700000000000002, 0.99999000000000005]
# mesginfo.P = [395.29849999999999, 0.0, 315.97555, 0.0, 0.0, 395.29849999999999, 238.45780999999999, 0.0, 0.0, 0.0, 1.0, 0.0]
# mesginfo.header.frame_id = 'mono_optical_frame'
mesginfo.header.stamp = mesgdata.header.stamp
# publish data/info
self.pub_vid_data[channel].publish(mesgdata)
self.pub_vid_info[channel].publish(mesginfo)
