-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathvision_sensor_streaming.py
132 lines (100 loc) · 4.12 KB
/
vision_sensor_streaming.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
import sim
import cv2 as cv
import numpy as np
import time
import array
from PIL import Image
from coppeliasim import CoppeliaSim, CoppeliaSensor
import matplotlib.pyplot as plt
# ============================================================= #
# Color Image Filtering :
# ============================================================= #
def filter_image(img, filterColor):
if filterColor == 'red':
lower_bound = np.array([0, 70, 50])
upper_bound = np.array([10, 255, 255])
elif filterColor == 'green':
lower_bound = np.array([36, 25, 25])
upper_bound = np.array([70, 255, 255])
elif filterColor == 'blue':
lower_bound = np.array([105, 70, 50])
upper_bound = np.array([130, 255, 255])
hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Create a mask using the bounds set
mask = cv.inRange(hsv, lower_bound, upper_bound)
# Create an inverse of the mask
mask_inv = cv.bitwise_not(mask)
# Filter only the red colour from the original image using the mask (foreground)
res = cv.bitwise_and(img, img, mask=mask)
# Filter the regions containing colours other than red from the grayscale image
background = cv.bitwise_and(gray, gray, mask=mask_inv)
# Convert the one channelled grayscale background to a three channelled image
background = np.stack((background,) * 3, axis=-1)
# add the foreground and the background
added_img = cv.add(res, background)
return mask, mask_inv, res, added_img, hsv
# ============================================================= #
# Orientation Detection :
# ============================================================= #
def detect_orientation(img):
# Convert image to grayscale:
#gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Convert image to binary:
mask, mask_inv, res, added_img, hsv = filter_image(img, 'green')
#ret, bw = cv.threshold(gray, 50, 255, cv.THRESH_BINARY | cv.THRESH_OTSU)
contours, res = cv.findContours(mask, cv.RETR_LIST, cv.CHAIN_APPROX_NONE)
for i, c in enumerate(contours):
# Calculate the area of each contour
area = cv.contourArea(c)
# Ignore contours that are too small or too large
if area < 100 or 100000 < area:
continue
# cv.minAreaRect returns:
# (center(x, y), (width, height), angle of rotation) = cv2.minAreaRect(c)
rect = cv.minAreaRect(c)
box = cv.boxPoints(rect)
box = np.intp(box)
# Retrieve the key parameters of the rotated bounding box
center = (int(rect[0][0]), int(rect[0][1]))
width = int(rect[1][0])
height = int(rect[1][1])
angle = int(rect[2])
if width < height:
angle = 90 - angle
else:
angle = -angle
#label = "(" + str(angle) + " deg"
label = "({x}, {y}, Angle={ori})".format(x=center[0], y=center[1], ori=angle)
#textbox = cv.rectangle(img, (center[0] - 35, center[1] - 25),
# (center[0] + 295, center[1] + 10), (255, 255, 255), -1)
cv.putText(img, label, (center[0], center[1]),
cv.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 1, cv.LINE_AA)
cv.drawContours(img, [box], 0, (0, 0, 255), 2)
cv.imshow('Output Image', img)
return img
mSim = CoppeliaSim()
mSim.connect(19997)
camera = CoppeliaSensor("Vision_sensor", "Vision")
#proximitySensor = CoppeliaSensor("Proximity_sensor", "Proximity")
time.sleep(2)
#es, image = camera.getImage()
#print(res)
#cap = cv.VideoCapture(0)
while sim.simxGetConnectionId(mSim.clientId != -1):
ret, res, image = camera.getImage()
if ret == sim.simx_return_ok:
#img = np.array(image, dtype=np.uint8)
img = np.array(image).astype(dtype=np.uint8)
img.resize([res[1], res[0], 3])
img2 = cv.cvtColor(img, cv.COLOR_RGB2BGR)
img2 = cv.flip(img2, 0)
a = detect_orientation(img2)
#_, frame = cap.read()
#cv.imshow('frame',frame)
#ret = proximitySensor.getProximityStatus()
#print(ret)
key = cv.waitKey(1) & 0xFF
if key == 27:
break
cv.destroyAllWindows()