116 lines
3.6 KiB
Python
116 lines
3.6 KiB
Python
|
# TBD: automatic thresholding, feature map (transformation), PLC library
|
||
|
|
#!/usr/bin/env python3
|
||
|
|
import sys
|
||
|
|
import argparse
|
||
|
|
import rospy
|
||
|
|
import time
|
||
|
|
import rospy
|
||
|
|
import cv2
|
||
|
|
import numpy as np
|
||
|
|
import time
|
||
|
|
|
||
|
|
from cv_bridge import CvBridge, CvBridgeError
|
||
|
|
from geometry_msgs.msg import Twist
|
||
|
|
from sensor_msgs.msg import Image
|
||
|
|
|
||
|
|
image_ros = True
|
||
|
|
|
||
|
|
max_value = 255
|
||
|
|
max_value_H = 360//2
|
||
|
|
low_H = 0
|
||
|
|
low_S = 0
|
||
|
|
low_V = 0
|
||
|
|
high_H = max_value_H
|
||
|
|
high_S = max_value
|
||
|
|
high_V = max_value
|
||
|
|
window_capture_name = 'Video Capture'
|
||
|
|
window_detection_name = 'Object Detection'
|
||
|
|
low_H_name = 'Low H'
|
||
|
|
low_S_name = 'Low S'
|
||
|
|
low_V_name = 'Low V'
|
||
|
|
high_H_name = 'High H'
|
||
|
|
high_S_name = 'High S'
|
||
|
|
high_V_name = 'High V'
|
||
|
|
bridge_object = CvBridge()
|
||
|
|
def on_low_H_thresh_trackbar(val):
|
||
|
|
global low_H
|
||
|
|
global high_H
|
||
|
|
low_H = val
|
||
|
|
low_H = min(high_H-1, low_H)
|
||
|
|
cv2.setTrackbarPos(low_H_name, window_detection_name, low_H)
|
||
|
|
def on_high_H_thresh_trackbar(val):
|
||
|
|
global low_H
|
||
|
|
global high_H
|
||
|
|
high_H = val
|
||
|
|
high_H = max(high_H, low_H+1)
|
||
|
|
cv2.setTrackbarPos(high_H_name, window_detection_name, high_H)
|
||
|
|
def on_low_S_thresh_trackbar(val):
|
||
|
|
global low_S
|
||
|
|
global high_S
|
||
|
|
low_S = val
|
||
|
|
low_S = min(high_S-1, low_S)
|
||
|
|
cv2.setTrackbarPos(low_S_name, window_detection_name, low_S)
|
||
|
|
def on_high_S_thresh_trackbar(val):
|
||
|
|
global low_S
|
||
|
|
global high_S
|
||
|
|
high_S = val
|
||
|
|
high_S = max(high_S, low_S+1)
|
||
|
|
cv2.setTrackbarPos(high_S_name, window_detection_name, high_S)
|
||
|
|
def on_low_V_thresh_trackbar(val):
|
||
|
|
global low_V
|
||
|
|
global high_V
|
||
|
|
low_V = val
|
||
|
|
low_V = min(high_V-1, low_V)
|
||
|
|
cv2.setTrackbarPos(low_V_name, window_detection_name, low_V)
|
||
|
|
def on_high_V_thresh_trackbar(val):
|
||
|
|
global low_V
|
||
|
|
global high_V
|
||
|
|
high_V = val
|
||
|
|
high_V = max(high_V, low_V+1)
|
||
|
|
cv2.setTrackbarPos(high_V_name, window_detection_name, high_V)
|
||
|
|
def camera_callback(data):
|
||
|
|
rospy.loginfo("callback started")
|
||
|
|
|
||
|
|
|
||
|
|
try:
|
||
|
|
# Select bgr8 because its the OpenCV encoding by default
|
||
|
|
cv_image = bridge_object.imgmsg_to_cv2(data, desired_encoding="passthrough")
|
||
|
|
#cv2.imshow("Original_image",cv_image)
|
||
|
|
except CvBridgeError as e:
|
||
|
|
print(e)
|
||
|
|
cv2.waitKey(10)
|
||
|
|
#ret1, cv_image = capture.read()
|
||
|
|
|
||
|
|
frame_HSV = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
|
||
|
|
frame_threshold = cv2.inRange(frame_HSV, (low_H, low_S, low_V), (high_H, high_S, high_V))
|
||
|
|
|
||
|
|
|
||
|
|
cv2.imshow(window_capture_name, cv_image)
|
||
|
|
cv2.imshow(window_detection_name, frame_threshold)
|
||
|
|
|
||
|
|
if not image_ros:
|
||
|
|
capture = cv2.VideoCapture("/home/andrew/Desktop/webcam_data/webcam/harvest2.mp4")
|
||
|
|
if (capture.isOpened()== False):
|
||
|
|
print("Error opening file.Please check path or name")
|
||
|
|
else:
|
||
|
|
bridge_object = CvBridge()
|
||
|
|
image_sub = rospy.Subscriber("/zed/zed_node/left/image_rect_color",Image,camera_callback) # rectified image from left camerarate = rospy.Rate(10) # 10hz
|
||
|
|
|
||
|
|
|
||
|
|
cv2.namedWindow(window_capture_name)
|
||
|
|
cv2.namedWindow(window_detection_name)
|
||
|
|
cv2.createTrackbar(low_H_name, window_detection_name , low_H, max_value_H, on_low_H_thresh_trackbar)
|
||
|
|
cv2.createTrackbar(high_H_name, window_detection_name , high_H, max_value_H, on_high_H_thresh_trackbar)
|
||
|
|
cv2.createTrackbar(low_S_name, window_detection_name , low_S, max_value, on_low_S_thresh_trackbar)
|
||
|
|
cv2.createTrackbar(high_S_name, window_detection_name , high_S, max_value, on_high_S_thresh_trackbar)
|
||
|
|
cv2.createTrackbar(low_V_name, window_detection_name , low_V, max_value, on_low_V_thresh_trackbar)
|
||
|
|
cv2.createTrackbar(high_V_name, window_detection_name , high_V, max_value, on_high_V_thresh_trackbar)
|
||
|
|
rospy.init_node('hsv_toolbar', anonymous=True)
|
||
|
|
rospy.loginfo("Node is starting ...")
|
||
|
|
|
||
|
|
|
||
|
|
#------------------#
|
||
|
|
while True:
|
||
|
|
time.sleep(0.05)
|
||
|
|
|
||
|
|
cv2.destroyAllWindows()
|