def debagify_msg(self, msg):
"""
The messages read from the rosbags do not have the correct classes so we need to reinitialize themself.
"""
if 'Header' in str(type(msg)):
newMsg = Header()
newMsg.seq = msg.seq
newMsg.stamp.secs = msg.stamp.secs
newMsg.stamp.nsecs = msg.stamp.nsecs
newMsg.frame_id = msg.frame_id
return newMsg
if 'RegionOfInterest' in str(type(msg)):
newMsg = RegionOfInterest()
newMsg.x_offset = msg.x_offset
newMsg.y_offset = msg.y_offset
newMsg.height = msg.height
newMsg.width = msg.width
newMsg.do_rectify = msg.do_rectify
return newMsg
if '_sensor_msgs__CompressedImage' in str(type(msg)):
im = self.bridge.compressed_imgmsg_to_cv2(msg).copy()
newMsg = self.bridge.cv2_to_compressed_imgmsg(im)
newMsg.header = self.debagify_msg(msg.header)
return newMsg
if '_sensor_msgs__CameraInfo' in str(type(msg)):
newMsg = CameraInfo()
newMsg.header = self.debagify_msg(msg.header)
newMsg.height = msg.height
newMsg.width = msg.width
newMsg.distortion_model = msg.distortion_model
newMsg.D = msg.D
newMsg.P = msg.P
newMsg.R = msg.R
newMsg.K = msg.K
newMsg.binning_x = msg.binning_x
newMsg.binning_y = msg.binning_y
newMsg.roi = self.debagify_msg(msg.roi)
return newMsg
raise Exception("Message type %s could not be debagified" %
str(type(msg)))
