<commit_before>#!/usr/bin/env python
##
# This utility is used for publishing
# OpenCV accessible camera images over
# ROS.
#
# Usage ./ros_camera OPENCV_ID
# where OPENCV_ID is a number >= 0
# representing opencv's index for 
# the particular device
#
import roslib
roslib.load_manifest('hrl_camera')
import sys
import rospy
import cv
import time
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge.cv_bridge import CvBridge, CvBridgeError
import hrl_lib.rutils as ru

class ROSImageClient:
    def __init__(self, topic_name):
        self.bridge = CvBridge()
        def message_extractor(ros_img):
            try:
                cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8')
                return cv_image
            except CvBridgeError, e:
                return None
        self.listener = ru.GenericListener('ROSImageClient', Image, topic_name, 
                                           .1, message_extractor)
    def get_frame(self):
        return self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False)

    def next(self):
        return self.get_frame()

class Prosilica(ROSImageClient):
    def __init__(self, camera_name, mode):
        self.camera_name = camera_name
        srv_name = '/%s/request_image' % camera_name
        topic_name = '/%s/image_rect_color' % camera_name
        ROSImageClient.__init__(self, topic_name)
        self.trigger_proxy = rospy.ServiceProxy(srv_name, ps.GetPolledImage)

    def get_frame(self):
        rq = ps.GetPolledImageRequest()
        rq.response_namespace = '/%s' % self.camera_name
        resp = self.trigger_proxy(rq)
        return self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False)


class ROSCamera(ROSImageClient):
    def __init__(self, topic_name):
        ROSImageClient.__init__(self, topic_name)

    def set_exposure(self, exposure):
        print 'ROSCamera: **** WARNING: set_exposure unimplemented over ROS ****'

    def set_frame_rate(self, rate):
        print 'ROSCamera: **** WARNING: set_frame_rate unimplemented over ROS ****'


class ROSStereoListener:
    def __init__(self, topics, rate=30.0, name='stereo_listener'):
        self.listener = ru.GenericListener(name, [Image, Image], topics, rate)
        self.lbridge = CvBridge()
        self.rbridge = CvBridge()

    def next(self):
        lros, rros =  self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)
        lcv = cv.CloneMat(self.lbridge.imgmsg_to_cv(lros, 'bgr8'))
        rcv = cv.CloneMat(self.rbridge.imgmsg_to_cv(rros, 'bgr8'))
        return lcv, rcv


if __name__ == '__main__':
    if len(sys.argv) < 2:
        print 'This utility is used for publishing'
        print 'OpenCV accessible camera images over'
        print 'ROS.\n'
        print 'Usage ./ros_camera OPENCV_ID'
        print 'where OPENCV_ID is a number >= 0'
        print 'representing opencv\'s index for '
        print 'the particular device'

    camera_id = int(sys.argv[1])
    topic_name = 'cvcamera' + str(camera_id)

    image_pub = rospy.Publisher(topic_name, Image)
    rospy.init_node('cvcamera', anonymous=True)

    capture = cv.CaptureFromCAM(camera_id)
    bridge = CvBridge()

    print 'Opening OpenCV camera with ID', camera_id
    print 'Publishing on topic', topic_name
    while not rospy.is_shutdown():
        try:
            cv_image = cv.CloneImage(cv.QueryFrame(capture))
            rosimage = bridge.cv_to_imgmsg(cv_image, "bgr8")
            image_pub.publish(rosimage)
        except rospy.exceptions.ROSSerializationException, e:
            print 'serialization exception'
        except CvBridgeError, e: 
            print e
            break
        except KeyboardInterrupt:
            print "Shutting down."
            break
        time.sleep(1/100.0)

    cv.DestroyAllWindows()
<commit_msg>added class to listen to camerainfo<commit_after>#!/usr/bin/env python
##
# This utility is used for publishing
# OpenCV accessible camera images over
# ROS.
#
# Usage ./ros_camera OPENCV_ID
# where OPENCV_ID is a number >= 0
# representing opencv's index for 
# the particular device
#
import roslib
roslib.load_manifest('hrl_camera')
import sys
import rospy
import cv
import time
from std_msgs.msg import String
from sensor_msgs.msg import Image
from sensor_msgs.msg import CameraInfo
from cv_bridge.cv_bridge import CvBridge, CvBridgeError
import hrl_lib.rutils as ru
import polled_camera.srv as ps
import numpy as np
import hrl_lib.tf_utils as tfu
import tf.transformations as tr

class ROSImageClient:
    def __init__(self, topic_name):
        self.bridge = CvBridge()
        def message_extractor(ros_img):
            try:
                cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8')
                return cv_image
            except CvBridgeError, e:
                return None
        self.listener = ru.GenericListener('ROSImageClient', Image, topic_name, 
                                           .1, message_extractor)
    def get_frame(self):
        return self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False)

    def next(self):
        return self.get_frame()

class Prosilica(ROSImageClient):
    def __init__(self, camera_name, mode):
        if mode == 'polled':
            srv_name = '/%s/request_image' % camera_name
            self.trigger_proxy = rospy.ServiceProxy(srv_name, ps.GetPolledImage)

        self.mode = mode
        self.camera_name = camera_name
        topic_name = '/%s/image_rect_color' % camera_name
        ROSImageClient.__init__(self, topic_name)

    def get_frame(self):
        if self.mode == 'polled':
            #will get a rospy.service.ServiceException if mode is wrong
            rq = ps.GetPolledImageRequest()
            rq.response_namespace = '/%s' % self.camera_name
            resp = self.trigger_proxy(rq)
        return self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False)

##
# from camera.py in laser_interface.
class ROSCameraCalibration:
    def __init__(self, channel):
        rospy.Subscriber(channel, CameraInfo, self.camera_info)
        self.has_msg = False

    def camera_info(self, msg):
        self.distortion = np.matrix(msg.D)
        self.K = np.reshape(np.matrix(msg.K), (3,3))
        self.R = np.reshape(np.matrix(msg.R), (3,3))
        self.P = np.reshape(np.matrix(msg.P), (3,4))
        self.w = msg.width
        self.h = msg.height
        self.frame = msg.header.frame_id
        self.has_msg = True

    ##
    # project 3D point into this camera 
    #   
    # @param p 3x1 matrix in given coord frame
    # @param tf_listener None if transformation not needed
    # @param from_frame None is default camera frame
    # @return 2x1 matrix
    def project(self, p, tf_listener=None, from_frame=None):
        if not(from_frame == None or from_frame == self.frame):
            p_cam = tfu.transform(self.frame, from_frame, tf_listener) \
                           * tfu.tf_as_matrix((p.A1.tolist(), tr.quaternion_from_euler(0,0,0)))
            trans, q = tfu.matrix_as_tf(p_cam)
            p = np.matrix(trans).T

        p = np.row_stack((p, np.matrix([1.])))
        pp = self.P * p
        pp = pp / pp[2,0]
        return pp[0:2,0]


class ROSCamera(ROSImageClient):
    def __init__(self, topic_name):
        ROSImageClient.__init__(self, topic_name)

    def set_exposure(self, exposure):
        print 'ROSCamera: **** WARNING: set_exposure unimplemented over ROS ****'

    def set_frame_rate(self, rate):
        print 'ROSCamera: **** WARNING: set_frame_rate unimplemented over ROS ****'


class ROSStereoListener:
    def __init__(self, topics, rate=30.0, name='stereo_listener'):
        self.listener = ru.GenericListener(name, [Image, Image], topics, rate)
        self.lbridge = CvBridge()
        self.rbridge = CvBridge()

    def next(self):
        lros, rros =  self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)
        lcv = cv.CloneMat(self.lbridge.imgmsg_to_cv(lros, 'bgr8'))
        rcv = cv.CloneMat(self.rbridge.imgmsg_to_cv(rros, 'bgr8'))
        return lcv, rcv


if __name__ == '__main__':
    if len(sys.argv) < 2:
        print 'This utility is used for publishing'
        print 'OpenCV accessible camera images over'
        print 'ROS.\n'
        print 'Usage ./ros_camera OPENCV_ID'
        print 'where OPENCV_ID is a number >= 0'
        print 'representing opencv\'s index for '
        print 'the particular device'

    camera_id = int(sys.argv[1])
    topic_name = 'cvcamera' + str(camera_id)

    image_pub = rospy.Publisher(topic_name, Image)
    rospy.init_node('cvcamera', anonymous=True)

    capture = cv.CaptureFromCAM(camera_id)
    bridge = CvBridge()

    print 'Opening OpenCV camera with ID', camera_id
    print 'Publishing on topic', topic_name
    while not rospy.is_shutdown():
        try:
            cv_image = cv.CloneImage(cv.QueryFrame(capture))
            rosimage = bridge.cv_to_imgmsg(cv_image, "bgr8")
            image_pub.publish(rosimage)
        except rospy.exceptions.ROSSerializationException, e:
            print 'serialization exception'
        except CvBridgeError, e: 
            print e
            break
        except KeyboardInterrupt:
            print "Shutting down."
            break
        time.sleep(1/100.0)

    cv.DestroyAllWindows()

{'fraction_non_alphanumeric': 0.09932088285229201, 'fraction_numerical': 0.00583616298811545, 'mean_word_length': 2.726769474100435, 'pattern_counts': {'":': 0, '<': 5, '<?xml version=': 0, '>': 7, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5703776', 'n_tokens_mistral': 3109, 'n_tokens_neox': 2932, 'n_words': 709}