Python sensor_msgs.msg.CameraInfo() Examples

def build_camera_info(self, attributes):  # pylint: disable=no-self-use
        """
        Private function to compute camera info

        camera info doesn't change over time
        """
        camera_info = CameraInfo()
        # store info without header
        camera_info.header = None
        camera_info.width = int(attributes['width'])
        camera_info.height = int(attributes['height'])
        camera_info.distortion_model = 'plumb_bob'
        cx = camera_info.width / 2.0
        cy = camera_info.height / 2.0
        fx = camera_info.width / (
            2.0 * math.tan(float(attributes['fov']) * math.pi / 360.0))
        fy = fx
        camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
        camera_info.D = [0, 0, 0, 0, 0]
        camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
        camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
        return camera_info 

def make_camera_msg(cam):
    camera_info_msg = CameraInfo()
    width, height = cam[0], cam[1]
    fx, fy = cam[2], cam[3]
    cx, cy = cam[4], cam[5]
    camera_info_msg.width = width
    camera_info_msg.height = height
    camera_info_msg.K = [fx, 0, cx,
                         0, fy, cy,
                         0, 0, 1]
                         
    camera_info_msg.D = [0, 0, 0, 0]
    
    camera_info_msg.P = [fx, 0, cx, 0,
                         0, fy, cy, 0,
                         0, 0, 1, 0]
    return camera_info_msg 

def __init__(self, camera_name, base_frame, table_height):
        """
        Initialize the instance

        :param camera_name: The camera name. One of (head_camera, right_hand_camera)
        :param base_frame: The frame for the robot base
        :param table_height: The table height with respect to base_frame
        """
        self.camera_name = camera_name
        self.base_frame = base_frame
        self.table_height = table_height

        self.image_queue = Queue.Queue()
        self.pinhole_camera_model = PinholeCameraModel()
        self.tf_listener = tf.TransformListener()

        camera_info_topic = "/io/internal_camera/{}/camera_info".format(camera_name)
        camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)

        self.pinhole_camera_model.fromCameraInfo(camera_info)

        cameras = intera_interface.Cameras()
        cameras.set_callback(camera_name, self.__show_image_callback, rectify_image=True) 

def _build_camera_info(self):
        """
        Private function to compute camera info

        camera info doesn't change over time
        """
        camera_info = CameraInfo()
        # store info without header
        camera_info.header = None
        camera_info.width = int(self.carla_actor.attributes['image_size_x'])
        camera_info.height = int(self.carla_actor.attributes['image_size_y'])
        camera_info.distortion_model = 'plumb_bob'
        cx = camera_info.width / 2.0
        cy = camera_info.height / 2.0
        fx = camera_info.width / (
            2.0 * math.tan(float(self.carla_actor.attributes['fov']) * math.pi / 360.0))
        fy = fx
        camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1]
        camera_info.D = [0, 0, 0, 0, 0]
        camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0]
        camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0]
        self._camera_info = camera_info

    # pylint: disable=arguments-differ 

def __init__(self, fovy=75, altitude=1.0, port=11311, env=None) :
		self.fovy = fovy
		self.altitude = altitude
		self.spawned = False
		self.port = port
		self.env = env
		if self.env is None :
			self.env = os.environ
		
		self.K = np.zeros((3,3))
		self.K[0][0] = self.fovy
		self.K[1][1] = self.fovy
		self.K[2][2] = 1.0
		self.K = np.matrix(self.K)
		self.P = np.matrix( np.concatenate( [self.K, np.zeros((3,1)) ], axis=1) )
		
		self.camerainfo = None
		self.listener_camera_info = rospy.Subscriber( '/CAMERA/camera_info', CameraInfo, self.callbackCAMERAINFO )
		self.image = None
		self.bridge = CvBridge()
		self.listener_image = rospy.Subscriber( '/CAMERA/image_raw', Image, self.callbackIMAGE ) 

def parse_yaml(filename):
    stream = file(filename, 'r')
    calib_data = yaml.load(stream)
    cam_info = CameraInfo()
    cam_info.width = calib_data['image_width']
    cam_info.height = calib_data['image_height']
    cam_info.K = calib_data['camera_matrix']['data']
    cam_info.D = calib_data['distortion_coefficients']['data']
    cam_info.R = calib_data['rectification_matrix']['data']
    cam_info.P = calib_data['projection_matrix']['data']
    cam_info.distortion_model = calib_data['distortion_model']
    cam_info.binning_x = calib_data['binning_x']
    cam_info.binning_y = calib_data['binning_y']
    cam_info.roi.x_offset = calib_data['roi']['x_offset']
    cam_info.roi.y_offset = calib_data['roi']['y_offset']
    cam_info.roi.height = calib_data['roi']['height']
    cam_info.roi.width = calib_data['roi']['width']
    cam_info.roi.do_rectify = calib_data['roi']['do_rectify']
    
    return cam_info 

def __init__(self, camera_name):
        self.left_cam_info_org = 0
        self.right_cam_info_org = 0

        # yamlファイルを読み込んでCameraInfoを返す
        left_file_name  = rospy.get_param('~left_file_name',  rospack.get_path('ps4eye')+'/camera_info/left.yaml')
        right_file_name = rospy.get_param('~right_file_name', rospack.get_path('ps4eye')+'/camera_info/right.yaml')
        self.left_cam_info = parse_yaml(left_file_name)
        self.right_cam_info = parse_yaml(right_file_name)
        left_topic = "/" + camera_name + "/left/camera_info"
        right_topic = "/" + camera_name + "/right/camera_info"
        # Timestampを合わせるためにsubする必要あり
        rospy.Subscriber("/null/left/camera_info", CameraInfo, self.leftCallback)
        rospy.Subscriber("/null/right/camera_info", CameraInfo, self.rightCallback)

        self.left_pub = rospy.Publisher(left_topic,CameraInfo) 
        self.right_pub = rospy.Publisher(right_topic,CameraInfo) 

def __init__(self, node_name='naoqi_camera'):
        NaoqiNode.__init__(self, node_name)

        self.camProxy = self.get_proxy("ALVideoDevice")
        if self.camProxy is None:
            exit(1)
        self.nameId = None
        self.camera_infos = {}
        def returnNone():
            return None
        self.config = defaultdict(returnNone)

        # ROS publishers
        self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=5)
        self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo, queue_size=5)

        # initialize the parameter server
        self.srv = Server(NaoqiCameraConfig, self.reconfigure)

        # initial load from param server
        self.init_config()

        # initially load configurations
        self.reconfigure(self.config, 0) 

def make_camera_msg(cam):
    camera_info_msg = CameraInfo()
    width, height = cam[0], cam[1]
    fx, fy = cam[2], cam[3]
    cx, cy = cam[4], cam[5]
    camera_info_msg.width = width
    camera_info_msg.height = height
    camera_info_msg.K = [fx, 0, cx,
                         0, fy, cy,
                         0, 0, 1]
                         
    camera_info_msg.D = [0, 0, 0, 0]
    
    camera_info_msg.P = [fx, 0, cx, 0,
                         0, fy, cy, 0,
                         0, 0, 1, 0]
    return camera_info_msg 

def __init__(self):
        # Get the camera parameters
        cam_info_topic = rospy.get_param('~camera/info_topic')
        camera_info_msg = rospy.wait_for_message(cam_info_topic, CameraInfo)
        self.cam_K = np.array(camera_info_msg.K).reshape((3, 3))

        self.img_pub = rospy.Publisher('~visualisation', Image, queue_size=1)
        rospy.Service('~predict', GraspPrediction, self.compute_service_handler)

        self.base_frame = rospy.get_param('~camera/robot_base_frame')
        self.camera_frame = rospy.get_param('~camera/camera_frame')
        self.img_crop_size = rospy.get_param('~camera/crop_size')
        self.img_crop_y_offset = rospy.get_param('~camera/crop_y_offset')
        self.cam_fov = rospy.get_param('~camera/fov')

        self.counter = 0
        self.curr_depth_img = None
        self.curr_img_time = 0
        self.last_image_pose = None
        rospy.Subscriber(rospy.get_param('~camera/depth_topic'), Image, self._depth_img_callback, queue_size=1)

        self.waiting = False
        self.received = False 

def subscribe(self):
        topic = "object_tracker/blob_info"
        self.centroid_sub = rospy.Subscriber(topic, BlobInfoArray, self.centroid_callback)
        self.pc_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.pc_callback)
        self.info_sub = rospy.Subscriber("/camera/rgb/camera_info", CameraInfo, self.info_callback) 

def start(self):
        """ Start the sensor """
        # initialize subscribers
        self._pointcloud_sub = rospy.Subscriber('/%s/depth/points' %(self.frame), PointCloud2, self._pointcloud_callback)
        self._camera_info_sub = rospy.Subscriber('/%s/left/camera_info' %(self.frame), CameraInfo, self._camera_info_callback)

        while self._camera_intr is None:
            time.sleep(0.1)
        
        self._running = True 

def rosmsg(self):
        """:obj:`sensor_msgs.CamerInfo` : Returns ROS CamerInfo msg 
        """
        from sensor_msgs.msg import CameraInfo, RegionOfInterest
        from std_msgs.msg import Header

        msg_header = Header()
        msg_header.frame_id = self._frame

        msg_roi = RegionOfInterest()
        msg_roi.x_offset = 0
        msg_roi.y_offset = 0
        msg_roi.height = 0
        msg_roi.width = 0
        msg_roi.do_rectify = 0

        msg = CameraInfo()
        msg.header = msg_header
        msg.height = self._height
        msg.width = self._width
        msg.distortion_model = 'plumb_bob'
        msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
        msg.K = [self._fx, 0.0, self._cx, 0.0, self._fy, self._cy, 0.0, 0.0, 1.0]
        msg.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
        msg.P = [self._fx, 0.0, self._cx, 0.0, 0.0, self._fx, self._cy, 0.0, 0.0, 0.0, 1.0, 0.0]
        msg.binning_x = 0
        msg.binning_y = 0
        msg.roi = msg_roi

        return msg 

def test_camera_info(self):
        """
        Tests camera_info
        """
        rospy.init_node('test_node', anonymous=True)
        msg = rospy.wait_for_message(
            "/carla/ego_vehicle/camera/rgb/front/camera_info", CameraInfo, timeout=TIMEOUT)
        self.assertEqual(msg.header.frame_id, "ego_vehicle/camera/rgb/front")
        self.assertEqual(msg.height, 600)
        self.assertEqual(msg.width, 800) 

def onStart(self):
        """ starting = start listening to depth images """
        self.sub_depth = rospy.Subscriber("/camera/depth_registered/image_raw", ImageMSG, self.incomingDepthData)
        self.cameraInfo = rospy.wait_for_message('/camera/depth_registered/camera_info', CamInfoMSG)
        self.cameraModel.fromCameraInfo(self.cameraInfo)
        rospy.loginfo("Camera info received") 

def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.front_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/image_raw',
            Image,
            queue_size=10)

        self.front_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/camera_info',
            CameraInfo,
            queue_size=10)

        self.bottom_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/image_raw',
            Image,
            queue_size=10)

        self.bottom_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/camera_info',
            CameraInfo,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            'odom',
            Odometry,
            queue_size=10) 

def __init__(self):

      # initialize ROS node and transform publisher
      rospy.init_node('crazyflie_detector', anonymous=True)
      self.pub_tf = tf.TransformBroadcaster()

      self.rate = rospy.Rate(50.0)                      # publish transform at 50 Hz

      # initialize values for crazyflie location on Kinect v2 image
      self.cf_u = 0                        # u is pixels left(0) to right(+)
      self.cf_v = 0                        # v is pixels top(0) to bottom(+)
      self.cf_d = 0                        # d is distance camera(0) to crazyflie(+) from depth image
      self.last_d = 0                      # last non-zero depth measurement

      # crazyflie orientation to Kinect v2 image (Euler)
      self.r = -1.5708
      self.p = 0
      self.y = -3.1415

      # Convert image from a ROS image message to a CV image
      self.bridge = CvBridge()

      cv2.namedWindow("KinectV2", 1)

      # Wait for the camera_info topic to become available
      rospy.wait_for_message('/kinect2/qhd/camera_info', CameraInfo)

      # Subscribe to Kinect v2 sd camera_info to get image frame height and width
      rospy.Subscriber('/kinect2/qhd/camera_info', CameraInfo, self.camera_data, queue_size=1)

      # Subscribe to registered color and depth images
      rospy.Subscriber('/kinect2/qhd/image_color_rect', Image, self.image_callback, queue_size=1)
      rospy.Subscriber('/kinect2/qhd/image_depth_rect', Image, self.depth_callback, queue_size=1)

      self.rate.sleep()                        # suspend until next cycle

   # This callback function sets parameters regarding the camera. 

def __callback_get_calibration_object(self, _):
        if not self.__calibration_object.is_set():
            return GetCalibrationCamResponse(False, CameraInfo())
        msg_camera_info = CameraInfo()
        mtx, dist = self.__calibration_object.get_camera_info()
        msg_camera_info.K = list(mtx.flatten())
        msg_camera_info.D = list(dist.flatten())
        return GetCalibrationCamResponse(True, msg_camera_info) 

def __init__(self):
        self.cv_bridge = CvBridge()
        self.camera_info_lock = threading.RLock()
        self.ar_tag_lock = threading.RLock()

        # Setup to control camera.
        self.joint_cmd_srv = rospy.ServiceProxy(JOINT_COMMAND_TOPIC, JointCommand)

        # Subscribe to camera pose and instrinsic streams.
        rospy.Subscriber(JOINT_STATE_TOPIC, JointState, self._camera_pose_callback)
        rospy.Subscriber(
            ROSTOPIC_CAMERA_INFO_STREAM, CameraInfo, self.camera_info_callback
        )
        self.img = None
        rospy.Subscriber(
            ROSTOPIC_CAMERA_IMAGE_STREAM,
            Image,
            lambda x: self.img_callback(x, "img", "bgr8"),
        )
        self.depth = None
        rospy.Subscriber(
            ROSTOPIC_CAMERA_DEPTH_STREAM,
            Image,
            lambda x: self.img_callback(x, "depth", None),
        )
        self.depth_registered = None
        rospy.Subscriber(
            ROSTOPIC_ALIGNED_CAMERA_DEPTH_STREAM,
            Image,
            lambda x: self.img_callback(x, "depth_registered", None),
        )
        rospy.Subscriber(ROSTOPIC_AR_POSE_MARKER, AlvarMarkers, self.alvar_callback)
        self.img = None
        self.ar_tag_pose = None

        self._transform_listener = TransformListener()
        self._update_camera_extrinsic = True
        self.camera_extrinsic_mat = None
        self.set_pan_pub = rospy.Publisher(ROSTOPIC_SET_PAN, Float64, queue_size=1)
        self.set_tilt_pub = rospy.Publisher(ROSTOPIC_SET_TILT, Float64, queue_size=1) 

def subscribe(self):
        topic = "/object_tracker/blob_info"
        self.centroid_sub = rospy.Subscriber(topic, BlobInfoArray, self.centroid_callback)
        topic = "/robot/range/"+self.limb+"_hand_range/state"
        self.ir_sub = rospy.Subscriber(topic, Range, self.ir_callback)
        topic = "/cameras/"+self.limb+"_hand_camera/camera_info"
        self.info_sub = rospy.Subscriber(topic, CameraInfo, self.info_callback) 

def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time):
    print("Exporting camera {}".format(camera))
    if kitti_type.find("raw") != -1:
        camera_pad = '{0:02d}'.format(camera)
        image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad))
        image_path = os.path.join(image_dir, 'data')
        image_filenames = sorted(os.listdir(image_path))
        with open(os.path.join(image_dir, 'timestamps.txt')) as f:
            image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines())
        
        calib = CameraInfo()
        calib.header.frame_id = camera_frame_id
        calib.width, calib.height = tuple(util['S_rect_{}'.format(camera_pad)].tolist())
        calib.distortion_model = 'plumb_bob'
        calib.K = util['K_{}'.format(camera_pad)]
        calib.R = util['R_rect_{}'.format(camera_pad)]
        calib.D = util['D_{}'.format(camera_pad)]
        calib.P = util['P_rect_{}'.format(camera_pad)]
            
    elif kitti_type.find("odom") != -1:
        camera_pad = '{0:01d}'.format(camera)
        image_path = os.path.join(kitti.sequence_path, 'image_{}'.format(camera_pad))
        image_filenames = sorted(os.listdir(image_path))
        image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
        
        calib = CameraInfo()
        calib.header.frame_id = camera_frame_id
        calib.P = util['P{}'.format(camera_pad)]
    
    iterable = zip(image_datetimes, image_filenames)
    bar = progressbar.ProgressBar()
    for dt, filename in bar(iterable):
        image_filename = os.path.join(image_path, filename)
        cv_image = cv2.imread(image_filename)
        calib.height, calib.width = cv_image.shape[:2]
        if camera in (0, 1):
            cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        encoding = "mono8" if camera in (0, 1) else "bgr8"
        image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
        image_message.header.frame_id = camera_frame_id
        if kitti_type.find("raw") != -1:
            image_message.header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))
            topic_ext = "/image_raw"
        elif kitti_type.find("odom") != -1:
            image_message.header.stamp = rospy.Time.from_sec(dt)
            topic_ext = "/image_rect"
        calib.header.stamp = image_message.header.stamp
        bag.write(topic + topic_ext, image_message, t = image_message.header.stamp)
        bag.write(topic + '/camera_info', calib, t = calib.header.stamp) 

def _broadcastCamera(self, camera, image_publisher, info_publisher):
        """
        INTERNAL METHOD, computes the image message and the info message of the
        given camera and publishes them into the ROS framework

        Parameters:
            camera - The camera used for broadcasting
            image_publisher - The ROS publisher for the Image message,
            corresponding to the image delivered by the active camera
            info_publisher - The ROS publisher for the CameraInfo message,
            corresponding to the parameters of the active camera
        """
        try:
            frame = camera.getFrame()
            assert frame is not None

            # Fill the camera info message
            info_msg = CameraInfo()
            info_msg.distortion_model = "plumb_bob"
            info_msg.header.frame_id = camera.getCameraLink().getName()
            info_msg.width = camera.getResolution().width
            info_msg.height = camera.getResolution().height
            info_msg.D = [0.0, 0.0, 0.0, 0.0, 0.0]
            info_msg.K = camera._getCameraIntrinsics()
            info_msg.R = [1, 0, 0, 0, 1, 0, 0, 0, 1]
            info_msg.P = list(info_msg.K)
            info_msg.P.insert(3, 0.0)
            info_msg.P.insert(7, 0.0)
            info_msg.P.append(0.0)

            # Fill the image message
            image_msg = self.image_bridge.cv2_to_imgmsg(frame)
            image_msg.header.frame_id = camera.getCameraLink().getName()

            # Check if the retrieved image is RGB or a depth image            
            if isinstance(camera, CameraDepth):
                image_msg.encoding = "16UC1"
            else:
                image_msg.encoding = "bgr8"

            # Publish the image and the camera info
            image_publisher.publish(image_msg)
            info_publisher.publish(info_msg)

        except AssertionError:
            pass 

def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.right_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/right/image_raw',
            Image,
            queue_size=10)

        self.right_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/right/camera_info',
            CameraInfo,
            queue_size=10)

        self.left_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/left/image_raw',
            Image,
            queue_size=10)

        self.left_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/left/camera_info',
            CameraInfo,
            queue_size=10)

        self.depth_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/image_raw',
            Image,
            queue_size=10)

        self.depth_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/camera_info',
            CameraInfo,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            'odom',
            Odometry,
            queue_size=10) 

def _initPublishers(self):
        """
        INTERNAL METHOD, initializes the ROS publishers
        """
        self.front_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/image_raw',
            Image,
            queue_size=10)

        self.front_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/front/camera_info',
            CameraInfo,
            queue_size=10)

        self.bottom_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/image_raw',
            Image,
            queue_size=10)

        self.bottom_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/bottom/camera_info',
            CameraInfo,
            queue_size=10)

        self.depth_cam_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/image_raw',
            Image,
            queue_size=10)

        self.depth_info_pub = rospy.Publisher(
            self.ros_namespace + '/camera/depth/camera_info',
            CameraInfo,
            queue_size=10)

        self.laser_pub = rospy.Publisher(
            self.ros_namespace + "/laser",
            LaserScan,
            queue_size=10)

        self.joint_states_pub = rospy.Publisher(
            '/joint_states',
            JointState,
            queue_size=10)

        self.odom_pub = rospy.Publisher(
            '/naoqi_driver/odom',
            Odometry,
            queue_size=10) 

def callback_step(data):
    global cmdx, cmdy, bridge
    obs, _, _, _ = env.step([cmdx, cmdy])
    rgb = obs["rgb_filled"]
    depth = obs["depth"].astype(np.float32)
    depth[depth > 10] = np.nan
    depth[depth < 0.45] = np.nan
    image_message = bridge.cv2_to_imgmsg(rgb, encoding="rgb8")
    depth_raw_image = (obs["depth"] * 1000).astype(np.uint16)
    depth_raw_message = bridge.cv2_to_imgmsg(depth_raw_image, encoding="passthrough")
    depth_message = bridge.cv2_to_imgmsg(depth, encoding="passthrough")

    now = rospy.Time.now()

    image_message.header.stamp = now
    depth_message.header.stamp = now
    depth_raw_message.header.stamp = now
    image_message.header.frame_id="camera_depth_optical_frame"
    depth_message.header.frame_id="camera_depth_optical_frame"
    depth_raw_message.header.frame_id="camera_depth_optical_frame"



    image_pub.publish(image_message)
    depth_pub.publish(depth_message)
    depth_raw_pub.publish(depth_raw_message)
    msg = CameraInfo(height=256, width=256, distortion_model="plumb_bob", D=[0.0, 0.0, 0.0, 0.0, 0.0],
                     K=[128, 0.0, 128.5, 0.0, 128, 128.5, 0.0, 0.0, 1.0],
                     R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
                     P=[128, 0.0, 128.5, -0.0, 0.0, 128, 128.5, 0.0, 0.0, 0.0, 1.0, 0.0])
    msg.header.stamp = now
    msg.header.frame_id="camera_depth_optical_frame"
    camera_info_pub.publish(msg)

    # odometry
    odom = env.get_odom()
    br.sendTransform((odom[0][0], odom[0][1], 0),
                         tf.transformations.quaternion_from_euler(0, 0, odom[-1][-1]),
                         rospy.Time.now(),
                         'base_footprint',
                         "odom")
    odom_msg = Odometry()
    odom_msg.header.stamp = rospy.Time.now()
    odom_msg.header.frame_id = 'odom'
    odom_msg.child_frame_id = 'base_footprint'

    odom_msg.pose.pose.position.x = odom[0][0]
    odom_msg.pose.pose.position.y = odom[0][1]
    odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y, odom_msg.pose.pose.orientation.z, \
        odom_msg.pose.pose.orientation.w = tf.transformations.quaternion_from_euler(0, 0, odom[-1][-1])

    odom_msg.twist.twist.linear.x = (cmdx + cmdy) * 5
    odom_msg.twist.twist.angular.z = (cmdy - cmdx) * 25

    odom_pub.publish(odom_msg) 

def save_camera_data(bag, kitti_type, kitti, util, bridge, camera, camera_frame_id, topic, initial_time):
    print("Exporting camera {}".format(camera))
    if kitti_type.find("raw") != -1:
        camera_pad = '{0:02d}'.format(camera)
        image_dir = os.path.join(kitti.data_path, 'image_{}'.format(camera_pad))
        image_path = os.path.join(image_dir, 'data')
        image_filenames = sorted(os.listdir(image_path))
        with open(os.path.join(image_dir, 'timestamps.txt')) as f:
            image_datetimes = map(lambda x: datetime.strptime(x[:-4], '%Y-%m-%d %H:%M:%S.%f'), f.readlines())
        
        calib = CameraInfo()
        calib.header.frame_id = camera_frame_id
        calib.width, calib.height = tuple(util['S_rect_{}'.format(camera_pad)].tolist())
        calib.distortion_model = 'plumb_bob'
        calib.K = util['K_{}'.format(camera_pad)]
        calib.R = util['R_rect_{}'.format(camera_pad)]
        calib.D = util['D_{}'.format(camera_pad)]
        calib.P = util['P_rect_{}'.format(camera_pad)]
            
    elif kitti_type.find("odom") != -1:
        camera_pad = '{0:01d}'.format(camera)
        image_path = os.path.join(kitti.sequence_path, 'image_{}'.format(camera_pad))
        image_filenames = sorted(os.listdir(image_path))
        image_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
        
        calib = CameraInfo()
        calib.header.frame_id = camera_frame_id
        calib.P = util['P{}'.format(camera_pad)]
    
    iterable = zip(image_datetimes, image_filenames)
    bar = progressbar.ProgressBar()
    for dt, filename in bar(iterable):
        image_filename = os.path.join(image_path, filename)
        cv_image = cv2.imread(image_filename)
        calib.height, calib.width = cv_image.shape[:2]
        if camera in (0, 1):
            cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        encoding = "mono8" if camera in (0, 1) else "bgr8"
        image_message = bridge.cv2_to_imgmsg(cv_image, encoding=encoding)
        image_message.header.frame_id = camera_frame_id
        if kitti_type.find("raw") != -1:
            image_message.header.stamp = rospy.Time.from_sec(float(datetime.strftime(dt, "%s.%f")))
            topic_ext = "/image_raw"
        elif kitti_type.find("odom") != -1:
            image_message.header.stamp = rospy.Time.from_sec(dt)
            topic_ext = "/image_rect"
        calib.header.stamp = image_message.header.stamp
        bag.write(topic + topic_ext, image_message, t = image_message.header.stamp)
        bag.write(topic + '/camera_info', calib, t = calib.header.stamp) 

def __init__(self, parent):
        super(KinectTracker, self).__init__(parent)
        self.name = self.__class__.__name__

        # Ros Stuff
        self.sub_depth = None #Initialised later
        self.bridge = CvBridge()
        self.pub_depth = rospy.Publisher("/camera/detector/image_raw", ImageMSG, queue_size=1)
        self.pub_camera = rospy.Publisher("/camera/detector/camera_info", CamInfoMSG, queue_size=1)


        # Parameters
        self.method = None
        self.setMethod(1)
        # Maximum depth we consider
        self.maxDepth = None
        self.setMaxDepth(m=4.5)
        # How far away we need to be from the background to be considered foreground
        self.bg_thresh = None
        self.setForegroundDist(cm=45)
        # Size of kernel for morphalogical operations
        self.kernel = None
        self.setKernel(2) #x*2+1
        # Number of morphalogical operation iterations
        self.morphIterations = None
        self.setIterations(1)
        # Camera matrix properties #TODO: use numpy and an intrinsic camera matrix to matrix-point multiply
        # FOR NOW USING ROS CAMERA MODEL
        #self.centerX = 319.5
        #self.centerY = 239.5
        #self.depthFocalLength = 525
        # Size restrictions of detected object (metric)
        self.estSize = None
        self.setSize(cm=6)
        self.estSizeTol = None
        self.setSizeTolerance(cm=4)
        self.priority = None # 0: blob size, 1:pixel coord, 2:x, 3:y, 4:z, 5: object width, 6:difference from background
        self.setPriority(0)
        self.depthEstMode = None
        self.setDepthEstMode(0)

        self.goal = [] # x,y,z in camera frame

        # Background History
        self.acc = None
        self.counterSteps = -1
        self.counter = 0
        #self.resetBackground(3.5)


        # Image buffers
        self.depth = None # current depth image [float32]
        self.show = None  # image to display and draw on [rgb8]
        self.method = 0 #0 = median, 1 = minimum

        self.cameraModel = image_geometry.PinholeCameraModel()
        self.cameraInfo = None 

def __init__(self,
          camera_depth_info_topic="camera/depth_registered/camera_info",
          camera_rgb_info_topic="camera/rgb/camera_info",
          camera_rgb_topic="camera/rgb/image",
          camera_depth_topic="camera/depth_registered/image",
          joints_topic="joint_states",
          messages_topic="/costar/messages",
          world_frame="/base_link",
          camera_frame="camera_depth_frame",
          ee_frame="/endpoint"):
        '''
        Define the listener manager. By default htese are listed as relative topics
        so they can be easily reconfigured via ROS command-line remapping.

        Parameters:
        ----------
        [REMOVED] camera_topic: topic on which RGB-D data is published
        camera_depth_info_topic:
        camera_rgb_info_topic:
        camera_rgb_topic:
        camera_depth_topic:
        joints_topic: topic on which we receive joints information from the robot
        messages_topic: 
        world_frame: TF frame which we should save all other parameters
                     relative to.
        camera_frame: name of the TF frame for the camera
        ee_frame: name of the end of the kinematic chain for the robot -- should
                  hopefully be the grasp frame, but can also be the wrist joint.
        '''
        self.T_world_ee = None
        self.T_world_camera = None
        self.camera_frame = camera_frame
        self.ee_frame = ee_frame

        self.q = None
        self.dq = None
        self.pc = None
        self.camera_depth_info = None
        self.camera_rgb_info = None

        self._camera_depth_info_sub = rospy.Subscriber(camera_depth_info_topic, CameraInfo, self._depthInfoCb)
        self._camera_rgb_info_sub = rospy.Subscriber(camera_rgb_info_topic, CameraInfo, self._rgbInfoCb)
        self._rgb_sub = rospy.Subscriber(camera_rgb_topic, Image, self._rgbCb)
        self._depth_sub = rospy.Subscriber(camera_depth_topic, Image, self._depthCb)
        self._joints_sub = rospy.Subscriber(joints_topic, JointState, self._jointsCb)
        self._messages_sub = rospy.Subscriber(joints_topic, JointState, self._jointsCb)
        
        self._resetData()

        self.num_trials = 0 

def __init__(self):
        self.hist_bins_q = rospy.get_param('~histogram/bins/quality')
        self.hist_bins_a = rospy.get_param('~histogram/bins/angle')

        self.dist_from_best_scale = rospy.get_param('~cost/dist_from_best_scale')
        self.dist_from_best_gain = rospy.get_param('~cost/dist_from_best_gain')
        self.dist_from_prev_view_scale = rospy.get_param('~cost/dist_from_prev_view_scale')
        self.dist_from_prev_view_gain = rospy.get_param('~cost/dist_from_prev_view_gain')

        self.height = (rospy.get_param('~height/z1'), rospy.get_param('~height/z2'))

        # Create a GridWorld where we will store values.
        self.gw_bounds = np.array([
            [rospy.get_param('~histogram/bounds/x1'), rospy.get_param('~histogram/bounds/y1')],
            [rospy.get_param('~histogram/bounds/x2'), rospy.get_param('~histogram/bounds/y2')]
        ])
        self.gw_res = rospy.get_param('~histogram/resolution')

        self.reset_gridworld(EmptySrv())
        self.hist_mean = 0
        self.fgw = GridWorld(self.gw_bounds, self.gw_res)
        self.fgw.add_grid('failures', 0.0)

        # Useful meshgrid for distance calculations
        xs = np.arange(self.gw.bounds[0, 0], self.gw.bounds[1, 0] - 1e-6, self.gw.res) + self.gw.res / 2
        ys = np.arange(self.gw.bounds[0, 1], self.gw.bounds[1, 1] - 1e-6, self.gw.res) + self.gw.res / 2
        self._xv, self._yv = np.meshgrid(xs, ys)

        # Get the camera parameters
        cam_info_topic = rospy.get_param('~camera/info_topic')
        camera_info_msg = rospy.wait_for_message(cam_info_topic, CameraInfo)
        self.cam_K = np.array(camera_info_msg.K).reshape((3, 3))

        self.img_pub = rospy.Publisher('~visualisation', Image, queue_size=1)
        rospy.Service('~update_grid', NextViewpoint, self.update_service_handler)
        rospy.Service('~reset_grid', EmptySrv, self.reset_gridworld)
        rospy.Service('~add_failure_point', AddFailurePoint, self.add_failure_point_callback)

        self.base_frame = rospy.get_param('~camera/base_frame')
        self.camera_frame = rospy.get_param('~camera/camera_frame')
        self.img_crop_size = rospy.get_param('~camera/crop_size')
        self.img_crop_y_offset = rospy.get_param('~camera/crop_y_offset')
        self.cam_fov = rospy.get_param('~camera/fov')

        self.counter = 0
        self.curr_depth_img = None
        self.curr_img_time = 0
        self.last_image_pose = None
        rospy.Subscriber(rospy.get_param('~camera/depth_topic'), Image, self._depth_img_callback, queue_size=1)