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)