ros_camera.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 ##
00003 # This utility is used for publishing
00004 # OpenCV accessible camera images over
00005 # ROS.
00006 #
00007 # Usage ./ros_camera OPENCV_ID
00008 # where OPENCV_ID is a number >= 0
00009 # representing opencv's index for 
00010 # the particular device
00011 #
00012 import roslib
00013 roslib.load_manifest('hrl_camera')
00014 import sys
00015 import rospy
00016 import cv
00017 import time
00018 from std_msgs.msg import String
00019 from sensor_msgs.msg import Image
00020 from sensor_msgs.msg import CameraInfo
00021 from cv_bridge.cv_bridge import CvBridge, CvBridgeError
00022 import hrl_lib.rutils as ru
00023 import polled_camera.srv as ps
00024 import numpy as np
00025 import hrl_lib.tf_utils as tfu
00026 import tf.transformations as tr
00027 
00028 class ROSImageClient:
00029     def __init__(self, topic_name):
00030         self.bridge = CvBridge()
00031         def message_extractor(ros_img):
00032             try:
00033                 cv_image = self.bridge.imgmsg_to_cv(ros_img, 'bgr8')
00034                 return cv_image, ros_img
00035             except CvBridgeError, e:
00036                 return None
00037         self.listener = ru.GenericListener('ROSImageClient', Image, topic_name, 
00038                                            .1, message_extractor)
00039     def get_frame(self, fresh=False):
00040         if not fresh:
00041             return self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)[0]
00042         else:
00043             not_fresh = True
00044             cur_time = rospy.Time.now().to_sec()
00045             while not_fresh:
00046                 img, rosmsg = self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)
00047                 if not (rosmsg.header.stamp.to_sec() < cur_time):
00048                     not_fresh = False
00049                 rospy.sleep(.1)
00050             return img
00051 
00052 
00053     def next(self):
00054         return self.get_frame()
00055 
00056 class Prosilica(ROSImageClient):
00057     def __init__(self, camera_name, mode):
00058         if mode == 'polled':
00059             srv_name = '/%s/request_image' % camera_name
00060             self.trigger_proxy = rospy.ServiceProxy(srv_name, ps.GetPolledImage)
00061 
00062         self.mode = mode
00063         self.camera_name = camera_name
00064         topic_name = '/%s/image_rect_color' % camera_name
00065         ROSImageClient.__init__(self, topic_name)
00066 
00067     def get_frame(self, fresh=False):
00068         if self.mode == 'polled':
00069             #will get a rospy.service.ServiceException if mode is wrong
00070             rq = ps.GetPolledImageRequest()
00071             rq.response_namespace = '/%s' % self.camera_name
00072             resp = self.trigger_proxy(rq)
00073         return ROSImageClient.get_frame(self, fresh)
00074 
00075         #if not fresh:
00076         #    return self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)[0]
00077 
00078         #else:
00079         #    not_fresh = True
00080         #    cur_time = rospy.Time.now().to_sec()
00081         #    while not_fresh:
00082         #        img, rosmsg = self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)
00083         #        if not (rosmsg.header.stamp.to_sec() < cur_time):
00084         #            not_fresh = False
00085         #        rospy.sleep(.1)
00086         #    return img
00087 
00088 
00089 
00090 
00091 
00092 ##
00093 # from camera.py in laser_interface.
00094 class ROSCameraCalibration:
00095     def __init__(self, channel=None, offline=False):
00096         if not offline:
00097             rospy.Subscriber(channel, CameraInfo, self.camera_info)
00098         self.has_msg = False
00099         self.msg = None
00100 
00101     def wait_till_msg(self):
00102         r = rospy.Rate(10)
00103         while not self.has_msg:
00104             r.sleep()
00105 
00106     def camera_info(self, msg):
00107         self.distortion = np.matrix(msg.D)
00108         self.K = np.reshape(np.matrix(msg.K), (3,3))
00109         self.R = np.reshape(np.matrix(msg.R), (3,3))
00110         self.P = np.reshape(np.matrix(msg.P), (3,4))
00111         self.w = msg.width
00112         self.h = msg.height
00113         self.frame = msg.header.frame_id
00114         self.has_msg = True
00115         self.msg = msg
00116 
00117     ##
00118     # project 3D point into this camera 
00119     #   
00120     # @param p 3x1 matrix in given coord frame
00121     # @param tf_listener None if transformation not needed
00122     # @param from_frame None is default camera frame
00123     # @return 2x1 matrix
00124     def project(self, p, tf_listener=None, from_frame=None):
00125         if not self.has_msg:
00126             raise RuntimeError('Has not been initialized with a CameraInfo message (call camera_info).')
00127         if not(from_frame == None or from_frame == self.frame):
00128             p_cam = tfu.transform(self.frame, from_frame, tf_listener) \
00129                            * tfu.tf_as_matrix((p.A1.tolist(), tr.quaternion_from_euler(0,0,0)))
00130             trans, q = tfu.matrix_as_tf(p_cam)
00131             p = np.matrix(trans).T
00132 
00133         hrow = np.matrix(np.zeros((1,p.shape[1])))
00134         p = np.row_stack((p, hrow))
00135         pp = self.P * p
00136         pp = pp / pp[2,:]
00137         return pp[0:2,:]
00138 
00139 
00140 class ROSCamera(ROSImageClient):
00141     def __init__(self, topic_name):
00142         ROSImageClient.__init__(self, topic_name)
00143 
00144     def set_exposure(self, exposure):
00145         print 'ROSCamera: **** WARNING: set_exposure unimplemented over ROS ****'
00146 
00147     def set_frame_rate(self, rate):
00148         print 'ROSCamera: **** WARNING: set_frame_rate unimplemented over ROS ****'
00149 
00150 
00151 class ROSStereoListener:
00152     def __init__(self, topics, rate=30.0, name='stereo_listener'):
00153         self.listener = ru.GenericListener(name, [Image, Image], topics, rate)
00154         self.lbridge = CvBridge()
00155         self.rbridge = CvBridge()
00156 
00157     def next(self):
00158         lros, rros =  self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True)
00159         lcv = cv.CloneMat(self.lbridge.imgmsg_to_cv(lros, 'bgr8'))
00160         rcv = cv.CloneMat(self.rbridge.imgmsg_to_cv(rros, 'bgr8'))
00161         return lcv, rcv
00162 
00163 
00164 if __name__ == '__main__':
00165     if len(sys.argv) < 2:
00166         print 'This utility is used for publishing'
00167         print 'OpenCV accessible camera images over'
00168         print 'ROS.\n'
00169         print 'Usage ./ros_camera OPENCV_ID'
00170         print 'where OPENCV_ID is a number >= 0'
00171         print 'representing opencv\'s index for '
00172         print 'the particular device'
00173 
00174     camera_id = int(sys.argv[1])
00175     topic_name = 'cvcamera' + str(camera_id)
00176 
00177     image_pub = rospy.Publisher(topic_name, Image)
00178     rospy.init_node('cvcamera', anonymous=True)
00179 
00180     capture = cv.CaptureFromCAM(camera_id)
00181     bridge = CvBridge()
00182 
00183     print 'Opening OpenCV camera with ID', camera_id
00184     print 'Publishing on topic', topic_name
00185     while not rospy.is_shutdown():
00186         try:
00187             cv_image = cv.CloneImage(cv.QueryFrame(capture))
00188             rosimage = bridge.cv_to_imgmsg(cv_image, "bgr8")
00189             image_pub.publish(rosimage)
00190         except rospy.exceptions.ROSSerializationException, e:
00191             print 'serialization exception'
00192         except CvBridgeError, e: 
00193             print e
00194             break
00195         except KeyboardInterrupt:
00196             print "Shutting down."
00197             break
00198         time.sleep(1/100.0)
00199 
00200     cv.DestroyAllWindows()


hrl_camera
Author(s): Hai Nguyen, Advait Jain, Cressel Anderson, Marc Killpack
autogenerated on Wed Nov 27 2013 11:37:01