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
