Python msg.CameraInfo方法代碼示例

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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 ) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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") 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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. 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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) 

# 需要導入模塊: from sensor_msgs import msg [as 別名]
# 或者: from sensor_msgs.msg import CameraInfo [as 別名]
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)