def findAllBlobs(clientId, youBotCam, homoMatrix):
currentDegree = 0
blobList = []
#we will rotate for 180 degree for spotting the blobs
i = 0
while (i < 4):
i += 1
err, res, image = vrep.simxGetVisionSensorImage(
clientId, youBotCam, 0, vrep.simx_opmode_buffer)
if err == vrep.simx_return_ok:
# do some image stuff ----------------------------------------------------------------------------------
cv2Image = convertToCv2Format(image, res)
tempBlobs = getBlobsGlobal(cv2Image, homoMatrix, clientId)
count = 0
for tb in tempBlobs:
count = 0
for b in blobList:
if move.isSamePoint(tb[0], b[0]) and tb[1] == b[1]:
count = count + 1
if count == 0:
print("added", tb, " to blobList")
blobList.append(tb)
move.rotate(10, clientId, True)
return blobList
def goAroundCorner(clientID, sensorHandles, rightSide, rayHit):
print("Going around corner: start")
# first drive forward -> rotate -> drive forward again
move.forward(0.8, clientID)
move.rotate(90, clientID, not rightSide)
move.forward(1.0, clientID)
# check if the bot has to do a uturn because of the corner or he can get back to following the obstacle
if (normalBorder(clientID, sensorHandles, rightSide)):
print("just a corner")
else:
print("U-Turn")
while True:
# when encountering the u-turn option drive forward by 0.2 as long as there is no obstacle which can be followed
# -> there is a obstacle to follow when more than 5 rays hit a obstacle
move.forward(0.2, clientID)
rangeData = rangeSensor.getSensorData(clientID, sensorHandles)
countRays = 0
for i in range(rayHit - 80, rayHit + 80):
if rangeData[i][3] < 0.8:
countRays += 1
if countRays < 5:
break
move.rotate(90, clientID, not rightSide)
move.forward(1.0, clientID)
wallOrient(clientID, sensorHandles, rayHit, False)
print("Going around corner: end")
def wallOrient(clientID, rangeSensorHandles, rayHit, isInOrientState):
print("WallOrient start")
# get sensor data from range sensors and the current bot orientation
rangeData = rangeSensor.getSensorData(clientID, rangeSensorHandles)
botOrient = move.getOrientation(clientID)
if (not isInOrientState):
print("Orient to nearest wall")
# to know how to align to the nearest wall on the side we
# use 2 rays to calculate the orientation of the wall -> x1, y2, and x2, y2
# start calc index of x2, y2 in rangeData
indexOfSndOrientPoint = rayHit + 10
for i in range(rayHit + 10, rayHit + 50):
if ((rangeData[i][3] - rangeData[rayHit][3]) < 0.5):
indexOfSndOrientPoint = i
break
# end calc index
x1 = rangeData[rayHit][0]
y1 = rangeData[rayHit][1]
x2 = rangeData[indexOfSndOrientPoint][0]
y2 = rangeData[indexOfSndOrientPoint][1]
# calculate the 2 possiblies of the 2 rays used so we can take the shorter ray, which
# will be our wanted target orientation
a1 = calcTargetOrient(clientID, x2, y2, x1, y1)
a2 = calcTargetOrient(clientID, x1, y1, x2, y2)
if abs(move.substractOrientation(botOrient, a1)) < abs(
move.substractOrientation(botOrient, a2)):
move.rotateUntilOrientation(clientID, a1)
isRight = True
else:
move.rotateUntilOrientation(clientID, a2)
isRight = False
else:
print("Turn because of a corner")
# Determine where the nearest wall on the left and right side is, to know in which direction to turn
if (rangeData[LEFT_RAY_NINETY][3] < 2):
move.rotate(90.0, clientID, True)
isRight = True
elif (rangeData[RIGHT_RAY_NINETY][3] < 2):
move.rotate(90.0, clientID, False)
isRight = False
print("WallOrient end")
return isRight
def driveAroundChair(clientID):
move.rotate(90.0, clientID, True)
move.forward(1.0, clientID)
move.rotate(90.0, clientID, False)
move.forward(1.0, clientID)
