hsv_threshold.py

#! /usr/bin/env python
import cv2
import rospy
import numpy as np
import argparse

from sensor_msgs.msg import Image

def callback_tb(value):
	pass

def setup_trackbars(range_filter):
	cv2.namedWindow("Trackbars", 0)

	for i in ["MIN", "MAX"]:
		v = 0 if i == "MIN" else 255

		for j in range_filter:
			cv2.createTrackbar("%s_%s" % (j, i), "Trackbars", v, 255, callback_tb)

def get_trackbar_values(range_filter):
	values = []

	for i in ["MIN", "MAX"]:
		for j in range_filter:
			v = cv2.getTrackbarPos("%s_%s" % (j, i), "Trackbars")
			values.append(v)

	return values

def callback(rgb):
	img = np.frombuffer(rgb.data, dtype=np.uint8).reshape(rgb.height, rgb.width, -1)
	blur = cv2.medianBlur(img, 5)
	hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
	v1_min, v2_min, v3_min, v1_max, v2_max, v3_max = get_trackbar_values('HSV')
	thresh = cv2.inRange(hsv, (v1_min, v2_min, v3_min), (v1_max, v2_max, v3_max))

	preview = cv2.bitwise_and(img, img, mask=thresh)
	cv2.imshow("Preview", preview)
	cv2.waitKey(3)


if __name__=="__main__":
	parser = argparse.ArgumentParser()

	parser.add_argument('-c', dest='topic', default='/tb2/camera/rgb/image_color', type=str, help='Topic from ROS')
	args = parser.parse_args(rospy.myargv()[1:])
	topic = args.topic

	print("Subscribing to %s" % (topic))

	setup_trackbars('HSV')

	rospy.init_node('hsv_threshold')

	image_sub = rospy.Subscriber(topic, Image, callback)

	rospy.spin()