import rosbag
import os
import argparse
import json

import rosbag
import rospy
import cv2
from time import time

from sensor_msgs.msg import Image

with rosbag.Bag('output.bag', 'w') as outbag:
    stamp = rospy.Time(0, 0)
    for topic, msg, t in rosbag.Bag('/home/daniela/bagfiles/camerabot/camerabot_real.bag').read_messages():
        if topic == "/world_camera/depth/image_raw" :
            # print(msg.header.stamp.secs, msg.header.stamp.nsecs)
            # outbag.write(topic, msg)
            stamp = rospy.Time(int(msg.header.stamp.secs), int(msg.header.stamp.nsecs))
        if topic == "/world_camera/rgb/image_raw" or topic == "/world_camera/rgb/camera_info" or topic == "/world_camera/depth/camera_info" or topic == "/world_camera/depth/image_raw":
            if stamp == rospy.Time(0, 0):
                continue
            else:
                # print(stamp)
                msg.header.stamp = stamp
                outbag.write(topic, msg, stamp)
        else:
            outbag.write(topic, msg)