00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
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
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
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
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
00119
00120
00121
00122
00123
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()