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)
