realsense_streamer = RealSense.RealSenseLib.RemoteRawCamera(ip) else: img_mode = True hue_filters = list() hs_filters = list() if os.path.isfile('markers_v1.json'): print "Loading marker information from markers_v1.json" hs_filters = ct.load_hs_filters('markers_v1.json', COLOR_MARGIN_HS) print hs_filters #else: print "Computing markers information from ideal hue distribution" colors = cg.get_hsv_equispaced_hues(NUM_COLORS) for color in colors: h,_,_ = color threshold = COLOR_MARGIN*(360.0/NUM_COLORS) h_min = 2*h - threshold/2 if h_min < 0: h_min += 360 h_min /= 2 h_max = 2*h + threshold/2 if h_max > 360: h_max -= 360 h_max /= 2
else: realsense_streamer = RealSense.RealSenseLib.RemoteRawCamera( ip) else: img_mode = True hue_filters = list() hs_filters = list() if os.path.isfile('markers_v1.json'): print "Loading marker information from markers_v1.json" hs_filters = ct.load_hs_filters('markers_v1.json', COLOR_MARGIN_HS) print hs_filters #else: print "Computing markers information from ideal hue distribution" colors = cg.get_hsv_equispaced_hues(NUM_COLORS) for color in colors: h, _, _ = color threshold = COLOR_MARGIN * (360.0 / NUM_COLORS) h_min = 2 * h - threshold / 2 if h_min < 0: h_min += 360 h_min /= 2 h_max = 2 * h + threshold / 2 if h_max > 360: h_max -= 360 h_max /= 2
def __init__(self): self.image_pub = rospy.Publisher("/RGB_img",Image) cv2.namedWindow("Image window", 1) self.bridge = CvBridge() self.image_quality = "sd" if self.image_quality == "qhd": self.image_height = 540; self.image_width = 960; elif self.image_quality == "hd": self.image_height = 1080; self.image_width = 1920; elif self.image_quality == "sd": self.image_height = 424; self.image_width = 512; self.image_topic = "/kinect2/" + self.image_quality + "/image_color_rect" self.depth_topic = "/kinect2/" + self.image_quality + "/image_depth_rect" self.cloud_topic = "/kinect2/" + self.image_quality + "/points" self.image_sub = rospy.Subscriber(self.image_topic,Image,self.callback_img) # self.depth_sub = rospy.Subscriber(self.depth_topic,Image,self.callback_depth) self.cloud_sub = rospy.Subscriber(self.cloud_topic,PointCloud2,self.callback_cloud) self.previous_time = time.clock() self.current_time = time.clock() self.cv_image = np.zeros((self.image_height,self.image_width,3), np.uint8) self.depth_image = np.zeros((self.image_height,self.image_width,1), np.uint8) self.hue_filters = list() self.hs_filters = list() self.points_rgb = np.zeros((0, 4), dtype=np.float32) # the return value of this algorithm is a list of centroids for the detected blobs self.centroids_xyz = np.zeros((0, 3), dtype=np.float32) self.cx = 0.0 self.cy = 0.0 self.h = 0.0 self.contour = None self.contour_group = [] self.cloud = PointCloud2() self.cloud_xyz = None self.centroid_xyz = [] self.command = None if os.path.isfile('markers_v1.json'): print "Loading marker information from markers_v1.json" hs_filters = ct.load_hs_filters('markers_v1.json', COLOR_MARGIN_HS) print hs_filters print "Computing markers information from ideal hue distribution" colors = cg.get_hsv_equispaced_hues(NUM_COLORS) for color in colors: h,_,_ = color threshold = COLOR_MARGIN*(360.0/NUM_COLORS) h_min = 2*h - threshold/2 if h_min < 0: h_min += 360 h_min /= 2 h_max = 2*h + threshold/2 if h_max > 360: h_max -= 360 h_max /= 2 self.hue_filters.append((int(round(h_min)), int(round(h_max)), int(round(h)))) print self.hue_filters