def main(): # create ros camera info messages l_info_manager = camera_info_manager.CameraInfoManager( cname=LEFT_CAM, url="file://%s/left.yaml" % CALIBRATION_DATA, namespace=LEFT_CAM) l_info_manager.loadCameraInfo() l_info_message = l_info_manager.getCameraInfo() r_info_manager = camera_info_manager.CameraInfoManager( cname=RIGHT_CAM, url="file://%s/right.yaml" % CALIBRATION_DATA, namespace=RIGHT_CAM) r_info_manager.loadCameraInfo() r_info_message = r_info_manager.getCameraInfo() # left camera publishers l_img_pub = rospy.Publisher("%s/image_raw" % LEFT_CAM, ImageMsg, queue_size=QUEUE_SIZE) l_info_pub = rospy.Publisher("%s/camera_info" % LEFT_CAM, CameraInfo, queue_size=QUEUE_SIZE) # right camera publishers r_img_pub = rospy.Publisher("%s/image_raw" % RIGHT_CAM, ImageMsg, queue_size=QUEUE_SIZE) r_info_pub = rospy.Publisher("%s/camera_info" % RIGHT_CAM, CameraInfo, queue_size=QUEUE_SIZE) # initialize node rospy.init_node("stereo_image_stream", anonymous=True) cap = cv2.VideoCapture(-1) bridge = CvBridge() while not rospy.is_shutdown(): # preprocess stereo images _, raw_image = cap.read() l_img, r_img = split_image(raw_image) rospy.loginfo("Read Cv Image: " + str(raw_image.shape)) # create ros image messages l_img_message = bridge.cv2_to_imgmsg(l_img, encoding="bgr8") r_img_message = bridge.cv2_to_imgmsg(r_img, encoding="bgr8") # set headers l_img_message.header.frame_id = HEADER_ID r_img_message.header.frame_id = HEADER_ID l_info_message.header.frame_id = HEADER_ID r_info_message.header.frame_id = HEADER_ID # publish messages l_img_pub.publish(l_img_message) r_img_pub.publish(r_img_message) l_info_pub.publish(l_info_message) r_info_pub.publish(r_info_message) cap.release() cv2.destroyAllWindows()
def __init__(self, hostname, username, password, frame_id, camera_info_url, prefix): self.hostname = hostname self.username = username self.password = password self.frame_id = frame_id self.camera_info_url = camera_info_url self.msg = CompressedImage() self.msg.header.frame_id = self.frame_id self.msg.format = "jpeg" self.valid = False self.updated = False # generate a valid camera name based on the hostname self.cname = camera_info_manager.genCameraName(self.hostname) self.cinfo = camera_info_manager.CameraInfoManager( cname=self.cname, url=self.camera_info_url, namespace=prefix) self.cinfo.loadCameraInfo() # required before getCameraInfo() self.st = StreamThread(self) self.st.start() self.pub = rospy.Publisher(prefix + "/image_raw/compressed", CompressedImage, self) self.caminfo_pub = rospy.Publisher(prefix + "/camera_info", CameraInfo, self) self.detect_dim()
def __init__(self): self.folder = rospy.get_param("~folder", "") self.encoding = rospy.get_param("~encoding", "bgr8") self.period = rospy.get_param("~update_period", 0.15) rospy.loginfo('update period {}'.format(self.period)) self.frame = rospy.get_param("~frame", "image_folder") self.camera_info_pub = None if True: self.camera_info_manager = camera_info_manager.CameraInfoManager( "tbd_camera_name") self.camera_info_url = rospy.get_param("~camera_info_url", "") if self.camera_info_url != "": rv = self.camera_info_manager.setURL(self.camera_info_url) if not rv: rospy.logerr("bad url " + self.camera_info_url) else: self.camera_info_manager.loadCameraInfo() self.camera_info_pub = rospy.Publisher("camera_info", CameraInfo, queue_size=1) self.bridge = CvBridge() self.image_pub = rospy.Publisher("image", Image, queue_size=3) self.image_list = [] self.image_ind = 1 self.timer = rospy.Timer(rospy.Duration(self.period), self.update)
def __init__(self, hostname, username, password, width, height, frame_id, camera_info_url, use_encrypted_password, camera): self.hostname = hostname self.username = username self.password = password self.width = width self.height = height self.frame_id = frame_id self.camera_info_url = camera_info_url self.use_encrypted_password = use_encrypted_password self.camera = camera # generate a valid camera name based on the hostname self.cname = camera_info_manager.genCameraName(self.hostname) self.cinfo = camera_info_manager.CameraInfoManager( cname=self.cname, url=self.camera_info_url) self.cinfo.loadCameraInfo() # required before getCameraInfo() self.st = None self.pub = rospy.Publisher("image_raw/compressed", CompressedImage, self, queue_size=1) self.caminfo_pub = rospy.Publisher("camera_info", CameraInfo, self, queue_size=1)
def rosSetup(self): """ Creates and setups ROS components """ self.cinfo = camera_info_manager.CameraInfoManager( cname=self.camera_model, url=self.camera_info_url, namespace=rospy.get_name()) self.cinfo.loadCameraInfo() # Mirar de cambiar por ImageTransport self.compressed_image_publisher = rospy.Publisher( "%scompressed" % rospy.get_namespace(), CompressedImage, self, queue_size=10) self.caminfo_publisher = rospy.Publisher("%scamera_info" % rospy.get_namespace(), CameraInfo, self, queue_size=10) # Sets the url self.url = 'http://%s/axis-cgi/mjpg/video.cgi?streamprofile=%s&camera=%d&fps=%d&compression=%d' % ( self.hostname, self.profile, self.camera_number, self.fps, self.compression) rospy.loginfo('Axis:rosSetup: Camera %s (%s:%d): url = %s, PTZ %s' % (self.camera_model, self.hostname, self.camera_number, self.url, self.ptz)) self.encodedstring = base64.encodestring(self.username + ":" + str(self.password))[:-1] self.auth = "Basic %s" % self.encodedstring # Diagnostic Updater self.diagnostics_updater = diagnostic_updater.Updater() self.diagnostics_updater.setHardwareID( "%s-%s:%s" % (self.camera_model, self.camera_id, self.hostname)) self.diagnostics_updater.add("Video stream updater", self.getStreamDiagnostic) #self.diagnostics_updater.add("Video stream frequency", self.getStreamFrequencyDiagnostic) if self.fps == 0: freq_bounds = {'min': 0.5, 'max': 100} else: freq_bounds = {'min': self.fps, 'max': self.fps} self.image_pub_freq = diagnostic_updater.HeaderlessTopicDiagnostic( "%scompressed" % rospy.get_namespace(), self.diagnostics_updater, diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.01, 1)) if self.ptz: # sets the ptz interface self.ptz_interface.rosSetup() self.diagnostics_updater.add("Ptz state updater", self.ptz_interface.getStateDiagnostic) # Creates a periodic callback to publish the diagnostics at desired freq self.diagnostics_timer = rospy.Timer(rospy.Duration(1.0), self.publishDiagnostics)
def main(): node_name = os.path.splitext(os.path.basename(__file__))[0] rospy.init_node(node_name) image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False) camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False) cim = camera_info_manager.CameraInfoManager( "picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") try: bridge = CvBridge() with picamera.PiCamera(framerate=90) as camera: camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT) with AnalyzeFlow(camera) as flow_analyzer: flow_analyzer.setup(camera.resolution) phase_analyzer = Localizer(camera, bridge) rospy.Subscriber('/pidrone/reset_transform', Empty, phase_analyzer.reset_callback) rospy.Subscriber('/pidrone/state', State, phase_analyzer.state_callback) camera.start_recording("/dev/null", format='h264', splitter_port=1, motion_output=flow_analyzer) print "Starting Flow" camera.start_recording(phase_analyzer, format='bgr', splitter_port=2) last_time = None while not rospy.is_shutdown(): camera.wait_recording(1 / 100.0) if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time: image_message = bridge.cv2_to_imgmsg( phase_analyzer.prev_img, encoding="bgr8") image_message.header.stamp = phase_analyzer.prev_rostime last_time = phase_analyzer.prev_rostime image_pub.publish(image_message) camera_info_pub.publish(cim.getCameraInfo()) camera.stop_recording(splitter_port=1) camera.stop_recording(splitter_port=2) print "Shutdown Received" except Exception: print "Camera Error!!" raise
def main(): rospy.init_node('flow_pub') image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False) camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False) cim = camera_info_manager.CameraInfoManager( "picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") try: velocity = axes_err() bridge = CvBridge() with picamera.PiCamera(framerate=90) as camera: camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT) with AnalyzeFlow(camera) as flow_analyzer: flow_analyzer.setup(camera.resolution) phase_analyzer = AnalyzePhase(camera, bridge) camera.start_recording("/dev/null", format='h264', splitter_port=1, motion_output=flow_analyzer) print "Starting Flow" camera.start_recording(phase_analyzer, format='bgr', splitter_port=2) last_time = None while not rospy.is_shutdown(): camera.wait_recording(1 / 100.0) if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time: image_message = bridge.cv2_to_imgmsg( phase_analyzer.prev_img, encoding="bgr8") image_message.header.stamp = phase_analyzer.prev_rostime # print "stamp", image_message.header.stamp last_time = phase_analyzer.prev_rostime image_pub.publish(image_message) camera_info_pub.publish(cim.getCameraInfo()) camera.stop_recording(splitter_port=1) camera.stop_recording(splitter_port=2) print "Shutdown Received" sys.exit() except Exception as e: raise
def __init__(self): self.ci = cim.CameraInfoManager(cname='my_camera', namespace='camera') self.image_sub = rospy.Subscriber("image_topic", Image, self.callback) rospy.init_node('CameraDriver', anonymous=False) self.pubInfo = rospy.Publisher('/camera/camera_info', CameraInfo, queue_size=10) self.pubImg = rospy.Publisher('/camera/image_raw', Image, queue_size=10)
def __init__(self, _prefix, camera_name, info_prefix, cinfo_url): self.name = camera_name self.uri = cinfo_url self.prefix = _prefix self.cname = camera_info_manager.genCameraName(self.prefix) rospy.loginfo("cname = " + self.cname) self.cinfo = camera_info_manager.CameraInfoManager( cname=_prefix, url=self.uri, namespace=info_prefix) self.caminfo_pub = rospy.Publisher(info_prefix + "/camera_info", CameraInfo, self) self.cinfo.loadCameraInfo() rospy.Subscriber(self.prefix + self.name, Image, self.image_callback)
def main(): node_name = os.path.splitext(os.path.basename(__file__))[0] rospy.init_node(node_name) image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False) camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False) cim = camera_info_manager.CameraInfoManager( "picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") try: bridge = CvBridge() with picamera.PiCamera(resolution=(CAMERA_WIDTH, CAMERA_HEIGHT), framerate=30) as camera: phase_analyzer = AnalyzePhase(camera, bridge) print "Starting Flow" camera.start_recording(phase_analyzer, format='bgr', splitter_port=1) last_time = None while not rospy.is_shutdown(): camera.wait_recording(1 / 30.0) if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time: image_message = bridge.cv2_to_imgmsg( phase_analyzer.prev_img, encoding="bgr8") image_message.header.stamp = phase_analyzer.prev_rostime # print "stamp", image_message.header.stamp last_time = phase_analyzer.prev_rostime image_pub.publish(image_message) camera_info_pub.publish(cim.getCameraInfo()) camera.stop_recording(splitter_port=1) print "Shutdown Recieved" sys.exit() except Exception as e: raise
def __init__(self, url): try: self.stream = urllib.urlopen(url) except: rospy.logerr('Unable to open camera stream: ' + str(url)) sys.exit() #'Unable to open camera stream') self.bytes = '' self.width = 640 self.height = 480 self.frame_id = 'camera' self.image_pub = rospy.Publisher("camera/image_raw", Image) self.cinfo = camera_info_manager.CameraInfoManager( cname='camera', url='PACKAGE://ipcam/config/calibration.yaml') self.cinfo.loadCameraInfo() # required before getCameraInfo() self.caminfo_pub = rospy.Publisher("camera/camera_info", CameraInfo) self.bridge = CvBridge()
def __init__(self): self.freq = rospy.get_param("~freq", DEFAULT_FREQUENCY) self.cname = rospy.get_param("~cname", DEFAULT_CNAME) self.url = rospy.get_param("~url", DEFAULT_URL) self.namespace = rospy.get_param("~namespace", DEFAULT_NAMESPACE) self.cam_manager = cim.CameraInfoManager(cname=self.cname, url=self.url, namespace=self.namespace) try: self.cam_manager.loadCameraInfo() rospy.loginfo("Camera name = %s", self.cam_manager.getCameraName()) except IOError as e: rospy.logerr( "Tried to load camera calibration data, but it is unreadable. Error = %s", e.message) rospy.signal_shutdown("Could not read calibration info") except cim.CameraInfoMissingError as e: rospy.logerr("Camera info missing. Error = %s", e.message) rospy.logerr( "Node will continue to run, but you will need to call set_camera_info service to fill out camera data" ) try: if not self.cam_manager.isCalibrated(): rospy.logwarn( "No exceptions during startup, but camera calibration data is using default values!" ) rospy.loginfo("Calibration info: ") print(self.cam_manager.getCameraInfo()) else: rospy.loginfo( "No errors during the loading of the camera data -- should be publishing okay." ) except cim.CameraInfoMissingError as e: rospy.logerr( "Even after trying to load camera info, we still don't have calibration information available" ) rospy.signal_shutdown( "Can't even provide default calibration data... shutting down") self.pub = rospy.Publisher("camera_info", cim.CameraInfo, queue_size=1) self.timer = rospy.Timer(rospy.Duration(1.0 / self.freq), self.timercb) return
def camera_driver(): pub = rospy.Publisher('camera_pi/image_raw', Image, queue_size=10) pub2 = rospy.Publisher('camera_pi/camera_info', CameraInfo, queue_size=10) rospy.init_node('camera_driver', anonymous=True) rate = rospy.Rate(24) picam = camera_info_manager.CameraInfoManager( cname='camera_pi', url='file:///home/p2p/.ros/camera_info/camera_pi.yaml', namespace='camera_pi') picam.loadCameraInfo() while not rospy.is_shutdown(): image = np.empty((h[1], h[0], 3), dtype=np.uint8).flatten() camera.capture(image, format='bgr', use_video_port=True) image = image.reshape([h[1], h[0], 3]) image = bridge.cv2_to_imgmsg(image, 'bgr8') pub.publish(image) pub2.publish(picam.getCameraInfo()) rate.sleep()
def __init__(self, url, config): # Logging if the camera is opened or not try: self.stream = urllib.urlopen(url) rospy.loginfo('Opened camera stream: ' + str(url)) except: rospy.logerr('Unable to open camera stream: ' + str(url)) sys.exit() #'Unable to open camera stream') # Initialize camera variables self.bytes = '' self.width = 640 self.height = 480 self.frame_id = 'camera' self.image_pub = rospy.Publisher("/camera/image_raw", Image) self.cinfo = camera_info_manager.CameraInfoManager(cname='camera', url=config) self.cinfo.loadCameraInfo() # required before getCameraInfo() self.caminfo_pub = rospy.Publisher("/camera/camera_info", CameraInfo) self.bridge = CvBridge()
def __init__(self, filename, frame_id, camera_info_url, prefix): self.filename = filename self.frame_id = frame_id self.camera_info_url = camera_info_url self.msg = CompressedImage() self.msg.header.frame_id = self.frame_id self.msg.format = "jpeg" im = cv2.imread(filename) self.width=im.shape[1] self.height=im.shape[0] f=open(filename, 'r+') self.msg.data = f.read() self.valid = True; self.updated = True; # generate a valid camera name based on the hostname self.cname = camera_info_manager.genCameraName(filename) self.cinfo = camera_info_manager.CameraInfoManager(cname = self.cname, url = self.camera_info_url, namespace = prefix) self.cinfo.loadCameraInfo() # required before getCameraInfo() self.st = None self.pub = rospy.Publisher(prefix + "/image_raw/compressed", CompressedImage, self) self.caminfo_pub = rospy.Publisher(prefix + "/camera_info", CameraInfo, self)
def main(): parser = argparse.ArgumentParser(description='Estimate the drone\'s pose with SLAM or localization') # Arguments to determine if the throttle command is being used. E.g.: # rosrun topic_tools throttle messages /pidrone/infrared 40.0 parser.add_argument('--SLAM', action='store_true', help=('Do you want to do SLAM?')) parser.add_argument('--offline', action='store_true', help=('Run offline?')) args = parser.parse_args() node_name = os.path.splitext(os.path.basename(__file__))[0] rospy.init_node(node_name) image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False) camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False) cim = camera_info_manager.CameraInfoManager("picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") try: bridge = CvBridge() with picamera.PiCamera(framerate=90) as camera: camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT) with AnalyzeFlow(camera) as flow_analyzer: flow_analyzer.setup(camera.resolution) if args.SLAM: if args.offline: phase_analyzer = MATL(camera, bridge) rospy.Subscriber('/pidrone/map', Empty, phase_analyzer.map_callback) else: phase_analyzer = SLAM(camera, bridge) else: phase_analyzer = Localizer(camera, bridge) rospy.Subscriber('/pidrone/reset_transform', Empty, phase_analyzer.reset_callback) rospy.Subscriber('/pidrone/state', State, phase_analyzer.state_callback) camera.start_recording("/dev/null", format='h264', splitter_port=1, motion_output=flow_analyzer) print "Starting Flow" camera.start_recording(phase_analyzer, format='bgr', splitter_port=2) last_time = None while not rospy.is_shutdown(): camera.wait_recording(1 / 100.0) if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time: image_message = bridge.cv2_to_imgmsg(phase_analyzer.prev_img, encoding="bgr8") image_message.header.stamp = phase_analyzer.prev_rostime last_time = phase_analyzer.prev_rostime image_pub.publish(image_message) camera_info_pub.publish(cim.getCameraInfo()) camera.stop_recording(splitter_port=1) camera.stop_recording(splitter_port=2) print "Shutdown Received" except Exception: print "Camera Error!!" raise
def reconfigure(self, new_config, level): """ Reconfigure the camera """ rospy.loginfo('reconfigure changed') if self.pub_img_.get_num_connections() == 0: rospy.loginfo( 'Changes recorded but not applied as nobody is subscribed to the ROS topics.' ) self.config.update(new_config) return self.config # check if we are even subscribed to a camera is_camera_new = self.nameId is None if is_camera_new: rospy.loginfo( 'subscribed to camera proxy, since this is the first camera') self.nameId = self.camProxy.subscribeCamera( "rospy_gvm", new_config['source'], new_config['resolution'], new_config['color_space'], new_config['frame_rate']) if self.config['source'] != new_config['source'] or is_camera_new: rospy.loginfo('updating camera source information') if new_config['source'] == kTopCamera: self.frame_id = self.config['camera_top_frame'] elif new_config['source'] == kBottomCamera: self.frame_id = self.config['camera_bottom_frame'] elif new_config['source'] == kDepthCamera: self.frame_id = self.config['camera_depth_frame'] else: rospy.logerr('Invalid source. Must be 0, 1 or 2') exit(1) # check if the camera changed if self.config['camera_info_url'] == "" or \ ( self.config['camera_info_url'] != new_config['camera_info_url'] and \ new_config['camera_info_url'] not in self.camera_infos ): self.load_camera_info() if 'cim' not in self.__dict__: self.cim = camera_info_manager.CameraInfoManager( cname='nao_camera') if not self.cim.setURL(new_config['camera_info_url']): rospy.logerr('malformed URL for calibration file') else: try: self.cim.loadCameraInfo() except IOExcept: rospy.logerr( 'Could not read from existing calibration file') if self.cim.isCalibrated(): rospy.loginfo('Successfully loaded camera info') self.camera_infos[ new_config['camera_info_url']] = self.cim.getCameraInfo() else: rospy.logerr( 'There was a problem loading the calibration file. Check the URL!' ) # set params camParams = self.extractParams(new_config) self.setParams(camParams) key_methods = [('resolution', 'setResolution'), ('color_space', 'setColorSpace'), ('frame_rate', 'setFrameRate')] if self.get_version() >= LooseVersion('2.0'): key_methods.append(('source', 'setActiveCamera')) for key, method in key_methods: if self.config[key] != new_config[key] or is_camera_new: self.camProxy.__getattribute__(method)(self.nameId, new_config[key]) self.config.update(new_config) return self.config
def main(): parser = argparse.ArgumentParser( description='Display Image Matching Regions') args = parser.parse_args() # [x, y, z, yaw] pos = [0, 0, 0, 0] angle_x = 0.0 angle_y = 0.0 # localization does not estimate z z = 0.33 first_locate = True #MS Local test, set to locate position immediately without waiting for web interface # locate_position = False locate_position = True map_counter = 0 max_map_counter = 0 # constant alpha_yaw = 0.1 # perceived yaw smoothing alpha hybrid_alpha = 0.7 # blend position with first frame and int # MS was 0.3 node_name = os.path.splitext(os.path.basename(__file__))[0] cim = camera_info_manager.CameraInfoManager( "picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") try: bridge = CvBridge() # Create the in-memory stream stream = io.BytesIO() with picamera.PiCamera(framerate=90) as camera: camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT) with picamera.array.PiRGBArray(camera) as stream: detector = cv2.ORB_create(nfeatures=NUM_FEATURES, scoreType=cv2.ORB_FAST_SCORE) map_kp, map_des, map_image = create_map('map.jpg') estimator = Single_Map_Localization(map_kp, map_des) estimator.set_map_image(map_image) print "Starting Single Map Localization" while True: # Do forever camera.capture(stream, format='bgr') # At this point the image is available as stream.array image = stream.array # Get a single image stream.seek(0) # Rewind the stream estimator.set_cur_image(image) curr_kp, curr_des = detector.detectAndCompute(image, None) img_keypoints = np.empty( (image.shape[0], image.shape[1], 3), dtype=np.uint8) cv2.drawKeypoints(image, curr_kp, img_keypoints) cv2.namedWindow('Current image', cv2.WINDOW_NORMAL) cv2.imshow('Current image', img_keypoints) cv2.waitKey(50) if curr_kp is not None and curr_des is not None: new_pos, weight = estimator.update( z, angle_x, angle_y, curr_kp, curr_des) # update position if new_pos is not None: pos = [ hybrid_alpha * new_pos[0] + (1.0 - hybrid_alpha) * pos[0], hybrid_alpha * new_pos[1] + (1.0 - hybrid_alpha) * pos[1], z, alpha_yaw * new_pos[3] + (1.0 - alpha_yaw) * pos[3] ] print '--pose', 'x ', pos[0], 'y ', pos[ 1], 'z', pos[2], 'yaw ', pos[3] map_counter = min(map_counter + 1, -MAX_BAD_COUNT) else: map_counter = map_counter - 1 # if it's been a while without a significant average weight if map_counter < MAX_BAD_COUNT: locate_position = False map_counter = 0 print 'Lost, leaving position control' print 'count', map_counter else: print "CANNOT FIND ANY FEATURES !!!!!" print "Shutdown Received" except Exception: print "Cancel Received" raise
self._frame = new_frame return Gst.FlowReturn.OK if __name__ == '__main__': rospy.init_node("bluerov_video_node") # Create the video object # Add port= if is necessary to use a different one video = Video(4777) bridge = cv_bridge.CvBridge() img_pub = rospy.Publisher('/camera/image', Image, queue_size=10) if pub_info: info_pub = rospy.Publisher('/camera/camera_info', CameraInfo, queue_size=10) info_manger = camera_info_manager.CameraInfoManager(cname='camera', namespace='camera') info_manger.loadCameraInfo() rate = rospy.Rate(10) while not rospy.is_shutdown(): # Wait for the next frame if not video.frame_available(): continue frame = video.frame() img_msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8") img_msg.header.frame_id = 'camera' img_msg.header.stamp = rospy.Time.now() img_pub.publish(img_msg)
def publish_image(): rospy.init_node('image_publisher') provider = rospy.get_param('~video_stream_provider') cap = cv2.VideoCapture(provider) if not cap.isOpened(): rospy.logerr('Failed to open video stream: {}'.format(provider)) return flip_horz = rospy.get_param('~flip_horizontal', default=False) flip_vert = rospy.get_param('~flip_vertical', default=False) if flip_horz and flip_vert: flip_code = -1 elif flip_horz: flip_code = 1 elif flip_vert: flip_code = 0 else: flip_code = None queue_size = rospy.get_param('~queue_size', default=10) ts = np.load(rospy.get_param('~ts_file')) ts += float(rospy.get_param('/trial_start_time', default=0.)) try: queue = ImageQueue(cap, ts, queue_size, flip_code) except IOError as e: rospy.logerr('Failed to initialize queue: {}'.format(e)) return camera_name = rospy.get_param('~camera_name') pub = rospy.Publisher(camera_name + '/image_raw', sensor_msgs.msg.Image, queue_size=1) bridge = cv_bridge.CvBridge() # Set up time information: either drawn directly from the /clock topic or offset from the current time use_manual_sim = rospy.get_param('~use_manual_sim', default=False) offset_from_start = rospy.get_param('~offset_from_start', default=False) if use_manual_sim: sim_clock = SimClockDelay() get_time = sim_clock.get_time elif offset_from_start: time_offset = rospy.Time.from_sec(ts[0]) - rospy.get_rostime() get_time = lambda: rospy.get_rostime() + time_offset else: get_time = rospy.get_rostime # Set up the camera calibration info camera_info_url = rospy.get_param('~camera_info_url', default='') camera_manager = camera_info_manager.CameraInfoManager(camera_name, camera_info_url) camera_manager.loadCameraInfo() # set up a camera_info publisher manually because image_transport doesn't exist in python :( # TODO: make this use SubscribeListener instead of a loop camera_info_pub = rospy.Publisher(camera_name + '/camera_info', sensor_msgs.msg.CameraInfo, queue_size=1) while not rospy.is_shutdown() and not queue.is_finished: if camera_info_pub.get_num_connections() > 0: camera_info_pub.publish(camera_manager.getCameraInfo()) try: t, img_raw = queue.get(block=True, timeout=1.) tm = rospy.Time.from_sec(t) cur_tm = get_time() if tm < cur_tm: rospy.logdebug("Video frame time has passed already (offset={}), discarding".format( (cur_tm - tm).to_sec())) continue # rospy.loginfo("got frame for time: {}".format(tm)) img = bridge.cv2_to_imgmsg(img_raw, encoding="bgr8") img.header.stamp = tm img.header.frame_id = camera_name if use_manual_sim: done = False while not done: try: sim_clock.wait(tm, timeout=1.) done = True except RuntimeError: pass else: while cur_tm < tm: sleep_len = min(tm - cur_tm, rospy.Duration(1)) # rospy.loginfo("Waiting {} to publish frame (total: {})".format(sleep_len.to_sec(), (tm - cur_tm).to_sec())) rospy.sleep(sleep_len) cur_tm = rospy.get_rostime() pub.publish(img) except Queue.Empty: pass except rospy.exceptions.ROSTimeMovedBackwardsException: rospy.loginfo('ROS time move backwards, assuming the bag restarted and reloading the queue') queue.stop() # rospy.loginfo('Queue stopped') cap = cv2.VideoCapture(provider) queue = ImageQueue(cap, ts, queue_size, flip_code)
def reconfigure(self, new_config, level): """ Reconfigure the camera """ if self.pub_img_.get_num_connections() == 0: rospy.loginfo( 'Changes recorded but not applied as nobody is subscribed to the ROS topics.' ) self.config.update(new_config) return self.config # check if we are even subscribed to a camera is_camera_new = self.nameId is None if self.config['source'] != new_config['source'] or is_camera_new: # unsubscribe for all zombie subscribers self.camProxy.unsubscribeAllInstances("rospy_gvm") # subscribe self.nameId = self.camProxy.subscribe("rospy_gvm", new_config['resolution'], new_config['color_space'], new_config['frame_rate']) rospy.loginfo('Using camera: %d . Subscriber name is %s .' % (new_config['source'], self.nameId)) if new_config['source'] == kTopCamera: self.frame_id = "/CameraTop_frame" elif new_config['source'] == kBottomCamera: self.frame_id = "/CameraBottom_frame" elif new_config['source'] == kDepthCamera: self.frame_id = new_config['camera3d_frame'] else: rospy.logerr('Invalid source. Must be 0, 1 or 2') exit(1) # check if the camera changed if self.config['camera_info_url'] != new_config['camera_info_url'] and \ new_config['camera_info_url'] and new_config['camera_info_url'] not in self.camera_infos: if 'cim' not in self.__dict__: self.cim = camera_info_manager.CameraInfoManager( cname='nao_camera') if not self.cim.setURL(new_config['camera_info_url']): rospy.logerr('malformed URL for calibration file') else: try: self.cim.loadCameraInfo() except IOExcept: rospy.logerr( 'Could not read from existing calibration file') if self.cim.isCalibrated(): rospy.loginfo('Successfully loaded camera info') self.camera_infos[ new_config['camera_info_url']] = self.cim.getCameraInfo() else: rospy.logerr( 'There was a problem loading the calibration file. Check the URL!' ) # set params for key, naoqi_key in [('source', kCameraSelectID), ('auto_exposition', kCameraAutoExpositionID), ('auto_exposure_algo', kCameraAecAlgorithmID), ('contrast', kCameraContrastID), ('saturation', kCameraSaturationID), ('hue', kCameraHueID), ('sharpness', kCameraSharpnessID), ('auto_white_balance', kCameraAutoWhiteBalanceID)]: if self.config[key] != new_config[key] or is_camera_new: self.camProxy.setParam(naoqi_key, new_config[key]) for key, naoqi_key, auto_exp_val in [ ('exposure', kCameraExposureID, 0), ('gain', kCameraGainID, 0), ('brightness', kCameraBrightnessID, 1) ]: if self.config[key] != new_config[key] or is_camera_new: self.camProxy.setParam(kCameraAutoExpositionID, auto_exp_val) self.camProxy.setParam(naoqi_key, new_config[key]) if self.config['white_balance'] != new_config[ 'white_balance'] or is_camera_new: self.camProxy.setParam(kCameraAutoWhiteBalanceID, 0) self.camProxy.setParam(kCameraWhiteBalanceID, new_config['white_balance']) for key, method in [('resolution', 'setResolution'), ('color_space', 'setColorSpace'), ('frame_rate', 'setFrameRate')]: if self.config[key] != new_config[key] or is_camera_new: self.camProxy.__getattribute__(method)(self.nameId, new_config[key]) self.config.update(new_config) return self.config
def main(): rospy.init_node('flow_pub') <<<<<<< HEAD ======= >>>>>>> 1d6eb550eab108dfc7c3e3b3b6fcca8777ced052 first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False) #image_mono_pub = rospy.Publisher("/pidrone/picamera/image_mono",Image, queue_size=1, tcp_nodelay=False) camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False) cim = camera_info_manager.CameraInfoManager("picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") global current_mode global phase_started global camera global phase_analyzer try: velocity = axes_err() bridge = CvBridge() with AnalyzeFlow(camera) as flow_analyzer: flow_analyzer.bridge = bridge
def __init__(self): NaoNode.__init__(self) rospy.init_node('nao_camera') self.camProxy = self.getProxy("ALVideoDevice") if self.camProxy is None: exit(1) # check if camera_switch is admissible self.camera_switch = rospy.get_param('~camera_switch', 0) if self.camera_switch == 0: self.frame_id = "/CameraTop_frame" elif self.camera_switch == 1: self.frame_id = "/CameraBottom_frame" else: rospy.logerr('Invalid camera_switch. Must be 0 or 1') exit(1) rospy.loginfo('using camera: %d', self.camera_switch) # Parameters for ALVideoDevice self.resolution = rospy.get_param('~resolution', kVGA) self.colorSpace = rospy.get_param('~color_space', kBGRColorSpace) self.fps = rospy.get_param('~fps', 30) # ROS publishers self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=10) self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo, queue_size=10) # initialize camera info manager self.cname = "nao_camera" # maybe it's a good idea to add _top and _bottom here ... self.cim = camera_info_manager.CameraInfoManager(cname=self.cname) self.calibration_file_bottom = rospy.get_param( '~calibration_file_bottom', None) self.calibration_file_top = rospy.get_param('~calibration_file_top', None) if self.camera_switch == 0: calibration_file = self.calibration_file_top else: calibration_file = self.calibration_file_bottom if not self.cim.setURL(calibration_file): rospy.logerr('malformed URL for calibration file') sys.exit(1) else: try: self.cim.loadCameraInfo() except IOExcept: rospy.logerr('Could not read from existing calibration file') exit(1) if self.cim.isCalibrated(): rospy.loginfo('Successfully loaded camera info') else: rospy.logerr( 'There was a problem loading the calibration file. Check the URL!' ) exit(1) # subscription to camProxy self.nameId = '' self.subscribe() # subscription to requests to start/stop publishing self.sub_publish = rospy.Subscriber(PUBLISH_STATUS_TOPIC, Bool, self.setPublishingStatus) # capture a dummy image self.dummyImage = self.camProxy.getImageRemote(self.nameId)
def main(): def reset_callback(data): """start localization when '/pidrone/reset_transform' is published to (press 'r')""" print "Start localization" global locate_position global map_counter global max_map_counter global first_locate locate_position = True first_locate = True map_counter = 0 max_map_counter = 0 first_image_pub.publish(map_message) parser = argparse.ArgumentParser( description='Display Image Matching Regions') args = parser.parse_args() # [x, y, z, yaw] pos = [0, 0, 0, 0] angle_x = 0.0 angle_y = 0.0 # localization does not estimate z z = 0.33 global first_locate #MS Local test, set to locate position immediately without waiting for web interface # locate_position = False global locate_position global map_counter global max_map_counter # constant alpha_yaw = 0.1 # perceived yaw smoothing alpha hybrid_alpha = 0.7 # blend position with first frame and int # MS was 0.3 node_name = os.path.splitext(os.path.basename(__file__))[0] rospy.init_node(node_name) image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False) camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False) posepub = rospy.Publisher('/pidrone/picamera/pose', PoseStamped, queue_size=1) posemsg = PoseStamped() first_image_pub = rospy.Publisher("/pidrone/picamera/first_image", Image, queue_size=1, latch=True) rospy.Subscriber('/pidrone/reset_transform', Empty, reset_callback) position_control_pub = rospy.Publisher('/pidrone/position_control', Bool, queue_size=1) cim = camera_info_manager.CameraInfoManager( "picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") try: bridge = CvBridge() # Create the in-memory stream stream = io.BytesIO() with picamera.PiCamera(framerate=90) as camera: camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT) with picamera.array.PiRGBArray(camera) as stream: detector = cv2.ORB_create(nfeatures=NUM_FEATURES, scoreType=cv2.ORB_FAST_SCORE) map_kp, map_des, map_image = create_map('map.jpg') map_message = bridge.cv2_to_imgmsg(map_image, encoding="bgr8") first_image_pub.publish(map_message) estimator = Single_Map_Localization(map_kp, map_des) print "Starting Single Map Localization" while not rospy.is_shutdown(): # Do until shutdown received camera.capture(stream, format='bgr') # At this point the image is available as stream.array image = stream.array # Get a single image stream.seek(0) # Rewind the stream image_message = bridge.cv2_to_imgmsg(image, encoding="bgr8") image_message.header.stamp = rospy.Time.now() # publish image for view in web interface image_pub.publish(image_message) curr_kp, curr_des = detector.detectAndCompute(image, None) if curr_kp is not None and curr_des is not None: new_pos, weight = estimator.update( z, angle_x, angle_y, curr_kp, curr_des) # update position if new_pos is not None: pos = [ hybrid_alpha * new_pos[0] + (1.0 - hybrid_alpha) * pos[0], hybrid_alpha * new_pos[1] + (1.0 - hybrid_alpha) * pos[1], z, alpha_yaw * new_pos[3] + (1.0 - alpha_yaw) * pos[3] ] print '--pose', 'x ', pos[0], 'y ', pos[ 1], 'z', pos[2], 'yaw ', pos[3] map_counter = min(map_counter + 1, -MAX_BAD_COUNT) # Publish the pose message to ROS posemsg.header.stamp = rospy.Time.now() posemsg.pose.position.x = pos[0] posemsg.pose.position.y = pos[1] posemsg.pose.position.z = pos[2] x, y, z, w = tf.transformations.quaternion_from_euler( 0, 0, pos[3]) posemsg.pose.orientation.x = x posemsg.pose.orientation.y = y posemsg.pose.orientation.z = z posemsg.pose.orientation.w = w posepub.publish(posemsg) else: map_counter = map_counter - 1 # if it's been a while without a significant average weight if map_counter < MAX_BAD_COUNT: locate_position = False map_counter = 0 position_control_pub.publish(False) print 'Lost, leaving position control, must Reset positon hold with "R"' print 'count', map_counter else: print "CANNOT FIND ANY FEATURES !!!!!" map_counter = map_counter - 1 print 'count', map_counter if map_counter < MAX_BAD_COUNT: locate_position = False map_counter = 0 position_control_pub.publish(False) print 'Lost, leaving position control, must Reset positon hold with "R"' print "Shutdown Received" except Exception: print "Cancel Received" raise
def reconfigure(self, new_config, level): """ Reconfigure the camera """ rospy.loginfo('reconfigure changed') if self.pub_img_.get_num_connections() == 0: rospy.loginfo( 'Changes recorded but not applied as nobody is subscribed to the ROS topics.' ) self.config.update(new_config) return self.config # check if we are even subscribed to a camera is_camera_new = self.nameId is None new_config['frame_rate'] = int(new_config['frame_rate']) if is_camera_new: rospy.loginfo( 'subscribed to camera proxy, since this is the first camera') if self.get_version() < LooseVersion('2.0'): # self.nameId = self.camProxy.subscribe("rospy_gvm", new_config['source'], # new_config['resolution'], new_config['color_space'], # int(new_config['frame_rate'])) self.nameId = self.camProxy.subscribe( "rospy_gvm", new_config['resolution'], new_config['color_space'], new_config['frame_rate']) else: self.nameId = self.camProxy.subscribeCamera( "rospy_gvm", new_config['source'], new_config['resolution'], new_config['color_space'], int(new_config['frame_rate'])) if self.config['source'] != new_config['source'] or is_camera_new: rospy.loginfo('updating camera source information') if new_config['source'] == kTopCamera: self.frame_id = "/CameraTop_frame" elif new_config['source'] == kBottomCamera: self.frame_id = "/CameraBottom_frame" elif new_config['source'] == kDepthCamera: self.frame_id = new_config['camera3d_frame'] else: rospy.logerr('Invalid source. Must be 0, 1 or 2') exit(1) # check if the camera changed if self.config['camera_info_url'] != new_config['camera_info_url'] and \ new_config['camera_info_url'] and new_config['camera_info_url'] not in self.camera_infos: if 'cim' not in self.__dict__: self.cim = camera_info_manager.CameraInfoManager( cname='nao_robot/camera/top') if not self.cim.setURL(new_config['camera_info_url']): rospy.logerr('malformed URL for calibration file') else: try: self.cim.loadCameraInfo() except IOExcept: rospy.logerr( 'Could not read from existing calibration file') if self.cim.isCalibrated(): rospy.loginfo('Successfully loaded camera info') self.camera_infos[ new_config['camera_info_url']] = self.cim.getCameraInfo() else: rospy.logerr( 'There was a problem loading the calibration file. Check the URL!' ) # set params key_naoqi_keys = [('auto_exposition', kCameraAutoExpositionID), ('auto_exposure_algo', kCameraAecAlgorithmID), ('contrast', kCameraContrastID), ('saturation', kCameraSaturationID), ('hue', kCameraHueID), ('sharpness', kCameraSharpnessID), ('auto_white_balance', kCameraAutoWhiteBalanceID)] #if self.get_version() < LooseVersion('2.0'): # key_method.append(('source', 'setActiveCamera')) for key, naoqi_key in key_naoqi_keys: if self.config[key] != new_config[key] or is_camera_new: if self.get_version() < LooseVersion('2.0'): self.camProxy.setParam(naoqi_key, new_config[key]) else: self.camProxy.setCamerasParameter(self.nameId, naoqi_key, new_config[key]) for key, naoqi_key, auto_exp_val in [ ('exposure', kCameraExposureID, 0), ('gain', kCameraGainID, 0), ('brightness', kCameraBrightnessID, 1) ]: if self.config[key] != new_config[key] or is_camera_new: if self.get_version() < LooseVersion('2.0'): self.camProxy.setParam(kCameraAutoExpositionID, auto_exp_val) self.camProxy.setParam(naoqi_key, new_config[key]) else: self.camProxy.setCamerasParameter(self.nameId, kCameraAutoExpositionID, auto_exp_val) self.camProxy.setCamerasParameter(self.nameId, naoqi_key, new_config[key]) if self.config['white_balance'] != new_config[ 'white_balance'] or is_camera_new: if self.get_version() < LooseVersion('2.0'): self.camProxy.setParam(kCameraAutoWhiteBalanceID, 0) #self.camProxy.setParam(kCameraWhiteBalanceID, new_config['white_balance']) else: self.camProxy.setCamerasParameter(self.nameId, kCameraAutoWhiteBalanceID, 0) self.camProxy.setCamerasParameter(self.nameId, kCameraWhiteBalanceID, new_config['white_balance']) key_methods = [('resolution', 'setResolution'), ('color_space', 'setColorSpace'), ('frame_rate', 'setFrameRate')] if self.get_version() >= LooseVersion('2.0'): key_methods.append(('source', 'setActiveCamera')) for key, method in key_methods: if self.config[key] != new_config[key] or is_camera_new: self.camProxy.__getattribute__(method)(self.nameId, new_config[key]) self.config.update(new_config) return self.config
def main(): """Publish a video as ROS messages. """ # Patse arguments. parser = argparse.ArgumentParser( description="Convert video into a rosbag.") parser.add_argument("video_file", help="Input video.") parser.add_argument("-c", "--camera", default="camera", help="Camera name.") parser.add_argument("-f", "--frame_id", default="camera", help="tf frame_id.") parser.add_argument("--width", type=np.int32, default="640", help="Image width.") parser.add_argument("--height", type=np.int32, default="480", help="Image height.") # parser.add_argument("--info_url", default="file:///eye_tracker_calib.yml", # help="Camera calibration url.") parser.add_argument("--info_url", default="file:///nao_bottom.yml", help="Camera calibration url.") args = parser.parse_args() print "Publishing %s." % (args.video_file) # Set up node. rospy.init_node("video_publisher", anonymous=True) img_pub = rospy.Publisher("/" + args.camera + "/image_raw", Image, queue_size=10) info_pub = rospy.Publisher("/" + args.camera + "/camera_info", CameraInfo, queue_size=10) info_manager = camera_info_manager.CameraInfoManager(cname=args.camera, url=args.info_url, namespace=args.camera) info_manager.loadCameraInfo() # Open video. video = cv2.VideoCapture(args.video_file) # Get frame rate. fps = video.get(cv2.CAP_PROP_FPS) rate = rospy.Rate(fps) # Loop through video frames. while not rospy.is_shutdown() and video.grab(): tmp, img = video.retrieve() if not tmp: print "Could not grab frame." break img_out = np.empty((args.height, args.width, img.shape[2])) # Compute input/output aspect ratios. aspect_ratio_in = np.float(img.shape[1]) / np.float(img.shape[0]) aspect_ratio_out = np.float(args.width) / np.float(args.height) if aspect_ratio_in > aspect_ratio_out: # Output is narrower than input -> crop left/right. rsz_factor = np.float(args.height) / np.float(img.shape[0]) img_rsz = cv2.resize(img, (0, 0), fx=rsz_factor, fy=rsz_factor, interpolation=cv2.INTER_AREA) diff = (img_rsz.shape[1] - args.width) / 2 img_out = img_rsz[:, diff:-diff - 1, :] elif aspect_ratio_in < aspect_ratio_out: # Output is wider than input -> crop top/bottom. rsz_factor = np.float(args.width) / np.float(img.shape[1]) img_rsz = cv2.resize(img, (0, 0), fx=rsz_factor, fy=rsz_factor, interpolation=cv2.INTER_AREA) diff = (img_rsz.shape[0] - args.height) / 2 img_out = img_rsz[diff:-diff - 1, :, :] else: # Resize image. img_out = cv2.resize(img, (args.width, args.height)) #print args.height #print args.width #print aspect_ratio_in #print aspect_ratio_out #print img_out.shape[0] #print img_out.shape[1] #assert img_out.shape[0:2] == (args.height, args.width) try: # Publish image. img_msg = bridge.cv2_to_imgmsg(img_out, "bgr8") img_msg.header.stamp = rospy.Time.now() img_msg.header.frame_id = args.frame_id img_pub.publish(img_msg) # Publish camera info. info_msg = info_manager.getCameraInfo() info_msg.header = img_msg.header info_pub.publish(info_msg) except CvBridgeError as err: print err rate.sleep() return