Code: Select all
# python
import numpy as np
import cv2 as cv
def readFrame():
sim.handleVisionSensor(self.handle)
buf, res = sim.getVisionSensorImg(self.handle)
return np.frombuffer(buf, dtype=np.uint8).reshape(*res, 3)
def showImage(k, img):
sim.setVisionSensorImg(sim.getObject(f':/{k}'), img.tobytes())
def sysCall_init():
sim = require('sim')
self.handle = sim.getObject('.')
robot = sim.getObject(':')
script = sim.getScript(sim.scripttype_childscript, robot)
self.robotScriptFuncs = sim.getScriptFunctions(script)
resX=sim.getObjectInt32Param(self.handle,sim.visionintparam_resolution_x)
resY=sim.getObjectInt32Param(self.handle,sim.visionintparam_resolution_y)
def sysCall_sensing():
m=sim.getObjectMatrix(self.handle)
res,packet1,packet2=sim.handleVisionSensor(self.handle)
if res>=0:
blobCnt=packet2[1]
valCnt=packet2[2]
for i in range(blobCnt):
blobPositionX=packet2[2+valCnt*i+3]
blobPositionY=packet2[2+valCnt*i+4]
depth=sim.getVisionSensorDepth(self.handle,1,{1+math.floor(blobPositionX*(resX-0.99)),1+math.floor(blobPositionY*(resY-0.99))},{1,1})
depth=sim.unpackFloatTable(depth,1)
print ("depth=",depth)
def sysCall_thread():
while True:
frame = readFrame()
imgw, imgh, imgc = frame.shape
blurred = cv.GaussianBlur(frame, (13, 13), 0)
hsv = cv.cvtColor(blurred, cv.COLOR_RGB2HSV)
# construct a mask for the color "red", then perform
# a series of dilations and erosions to remove any small
# blobs left in the mask
mask = cv.inRange(hsv, (0, 200, 64), (80, 255, 200))
n = 5
mask = cv.dilate(mask, None, iterations=n)
mask = cv.erode(mask, None, iterations=n)
maskdbg = cv.cvtColor(mask, cv.COLOR_GRAY2RGB)
contours, hierarchy = cv.findContours(mask.copy(), cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
if contours:
c = max(contours, key=cv.contourArea)
# Get the minimum enclosing circle for the contour
((x, y), radius) = cv.minEnclosingCircle(c)
center = (int(x), int(y))
radius = int(radius)
cv.circle(maskdbg, center, radius, (0, 255, 0), 2)
cv.arrowedLine(maskdbg, center, (imgw//2, imgh//2), (255, 0, 0), 2)
#print ("radius=",radius)
# Calculate the normalized center coordinates
normalize = lambda x: np.interp(x, (0, imgw), (-1, 1))
x_normalized, y_normalized = map(normalize, (x, y))
# Set the box center using the provided function from the robot script
self.robotScriptFuncs.setBoxCenter(x_normalized, y_normalized)
#if contours:
#c = max(contours, key = cv.contourArea)
#x, y, w, h = cv.boundingRect(c)
#cv.rectangle(maskdbg, (x, y), (x+w, y+h), (0, 255, 0), 2)
#cv.arrowedLine(maskdbg, (x+w//2, y+h//2), (imgw//2, imgh//2), (255, 0, 0), 2)
# normalize rect in -1 ... +1:
#normalize = lambda x: np.interp(x[0], (0, x[1]), (-1, 1))
#x, y, w, h = map(normalize, ((x, imgw), (y, imgh), (w, imgw), (h, imgh)))
# box center:
#self.robotScriptFuncs.setBoxCenter(x , y )
showImage('blurred', blurred)
showImage('hsv', hsv)
showImage('mask', maskdbg)