Go to the documentation of this file.00001 import roslib
00002 roslib.load_manifest('laser_interface')
00003 import laser_interface.camera as cam
00004 from laser_interface.laser_detector import *
00005 import rospy
00006 import cv
00007
00008 stereo_camera = '/wide_stereo'
00009 image_type = 'image_rect_color'
00010 coi = 1
00011
00012 stereo = cam.ROSStereoListener([stereo_camera + '/left/' + image_type, stereo_camera + '/right/' + image_type])
00013 sample_frame, r = stereo.next()
00014 threshold = [100, 240]
00015
00016 splitter = SplitColors(sample_frame)
00017 intensity_filter = BrightnessThreshold(sample_frame, thres_low=threshold[0], thres_high=threshold[1], tune=True)
00018
00019
00020 cv.NamedWindow('low', 1)
00021 cv.NamedWindow('high', 1)
00022 cv.NamedWindow('combined', 1)
00023 cv.NamedWindow('original', 1)
00024
00025 def low_thresh_func(val):
00026 threshold[0] = val
00027 intensity_filter.set_thresholds(threshold)
00028 print 'low', val
00029
00030 def high_thresh_func(val):
00031 threshold[1] = val
00032 intensity_filter.set_thresholds(threshold)
00033 print 'high', val
00034
00035 cv.CreateTrackbar('low_threshold', 'low', threshold[0], 255, low_thresh_func)
00036 cv.CreateTrackbar('high_threshold', 'high', threshold[1], 255, high_thresh_func)
00037
00038
00039 while not rospy.is_shutdown():
00040 image, _ = stereo.next()
00041 r, g, b = splitter.split(image)
00042 filtered_image = intensity_filter.threshold(g)
00043 cv.ShowImage('low', intensity_filter.thresholded_low)
00044 cv.ShowImage('high', intensity_filter.thresholded_high)
00045
00046 cv.ShowImage('combined', filtered_image)
00047 cv.WaitKey(33)
00048
00049
00050