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 normalBorder(clientID, sensorHandles, rayHit):
rangeData = rangeSensor.getSensorData(clientID, sensorHandles)
for i in range(rayHit - 15, rayHit + 15):
for j in range(i + 5, i + 30):
# u-turn encountered when distance difference is greater than 0.2 for at least 2 points
if (abs(rangeData[i][3] - rangeData[j][3]) > 0.2):
return False
return True
def detectCorner(clientID, rangeSensorHandles, rayHit, maxDist):
# get the current data from the range sensors
rangeData = rangeSensor.getSensorData(clientID, rangeSensorHandles)
# a corner is encountered when at least one distance of the rangeData rays exceeds the max distant by 0.5
for i in range(rayHit - 5, rayHit + 5):
if (rangeData[i][3] > maxDist + 0.5):
print("corner detected!")
return True
return False
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 calcFreeSpace(clientID, sensorHandles):
# get sensor data and the (calculated) ray index of the ray to the target which is used for rangeData
rangeData = rangeSensor.getSensorData(clientID, sensorHandles)
ray = getRayToTarget(clientID)
# non trackable distance if ray index to target (index of range sensor data) is above 633 or below 50
if ray < 50 or ray > 633:
# return false value (-1.0 is interpreted as
return -1.0
minDistToObstacle = rangeData[ray][3]
# search for a smaller distance in rangeData in target direction
for i in range(ray - 50, ray + 50):
if rangeData[i][3] < minDistToObstacle:
minDistToObstacle = rangeData[i][3]
return minDistToObstacle
def forwardUntilObstacleAnywhere(targetPos, clientID, rangeSensorHandles):
# set velocety to 0
move.setWheelVelocity(clientID, 0)
# start moving
move.startMoving(clientID)
# continuously check traveled distance
distance = 0.0
dt = 0.0
maxFalseAngle = 5
x, y = move.getPos(clientID)[0][:-1]
xCurrent, yCurrent = move.getPos(clientID)[0][:-1]
stop = False
hit = 0
while not isSamePoint(targetPos, [xCurrent, yCurrent]) and stop != True:
# get range sensor data as list of x,y,z,distance
rangeData = rangeSensor.getSensorData(clientID, rangeSensorHandles)
# range sensor angle is estimated for about 250 degrees
#for i in range(95, 588): #95 and 588 are estimated values for data from range sensor which are between -90 and 90 degrees (0 is front)
for i in range(
75, 602
): #95 and 588 are estimated values for data from range sensor which are between -90 and 90 degrees (0 is front)
if rangeData[i][3] <= 0.3:
stop = True
hit = i
break
time.sleep(
1.0e-06
) # problems with very small time slices -> little delay (if you have a bad angle calculation on your pc try to change this value)
end = time.time()
xCurrent, yCurrent = move.getPos(clientID)[0][:-1]
targetOrientation = move.calcTargetOrient(xCurrent, yCurrent,
targetPos[0], targetPos[1])
if abs(move.getOrientation(clientID) -
targetOrientation) > maxFalseAngle:
rotateUntilOrientation(clientID, targetOrientation)
move.startMoving(clientID)
# stop moving
move.setWheelVelocity(clientID, 0)
return (stop, hit)
def distB(clientID, sensorHandles, goalName):
isNotGoal = True
while (isNotGoal):
isNotGoal, hitRay = headTowardsModel(clientID, goalName, sensorHandles)
# if headTowardsModel returned False it means it successfully got to the goal point
if (not isNotGoal):
print("FINISHED")
return
# if there is a chair in front -> go around chair
if (isChairInFront(rangeSensor.getSensorData(clientID,
sensorHandles))):
driveAroundChair(clientID)
# if there is no chair and headTowardsModel returned True -> follow the boundary
else:
# orient to wall to have a better start when following a wall
isRight = wallOrient(clientID, sensorHandles, hitRay, False)
followBoundary(clientID, sensorHandles, isRight)
print("Bot is in goal: {}".format(not isNotGoal))
def forwardUntilObstacleAnywhere(meter, clientID, rangeSensorHandles):
# set velocety to 0
setWheelVelocity(clientID, 0)
# start moving
startMoving(clientID)
# continuously check traveled distance
distance = 0.0
dt = 0.0
x = 0.0
y = 0.0
stop = False
hit = 0
while distance <= meter and stop != True:
start = time.time()
# get range sensor data as list of x,y,z,distance
rangeData = rangeSensor.getSensorData(clientID, rangeSensorHandles)
# range sensor angle is estimated for about 250 degrees
for i in range(
95, 588
): #95 and 588 are estimated values for data from range sensor which are between -90 and 90 degrees (0 is front)
if rangeData[i][3] <= 0.3:
stop = True
hit = i
break
x, y, w = odometry(x, y, 0.0, FORWARD_VEL, 0.0, 0.0, dt)
distance = math.sqrt(x * x + y * y)
time.sleep(
1.0e-06
) # problems with very small time slices -> little delay (if you have a bad angle calculation on your pc try to change this value)
end = time.time()
dt = end - start
# stop moving
setWheelVelocity(clientID, 0)
return (stop, hit)
def followObstacle(clientID, sensorHandles, rightSide, roundFinished):
print("Follow boundary: start")
# set rayHit to the ray index where the followed wall is
if rightSide:
rayHit = LEFT_RAY_NINETY
else:
rayHit = RIGHT_RAY_NINETY
targetDistanceToWall = 0.5
minRange = targetDistanceToWall - 0.05
maxRange = targetDistanceToWall + 0.05
counter = 0
startPoint = (move.getPos(clientID)[0][0], move.getPos(clientID)[0][1]
) #point where ubot first hit the obstacle
res, objHandle = vrep.simxGetObjectHandle(clientID, 'Goal',
vrep.simx_opmode_oneshot_wait)
RES, targetPosition = vrep.simxGetObjectPosition(
clientID, objHandle, -1, vrep.simx_opmode_oneshot_wait)
goalPoint = (targetPosition[0], targetPosition[1])
minDist = move.getDistanceBetweenPoints(
startPoint, goalPoint) #shortest dist to the goal
minPoint = startPoint #coordinates of the nearest point to the goal
disToMin = 0
while True:
move.startMoving(clientID)
#variables for distance tracking
x = 0.0
y = 0.0
dt = 0.0
while not detectCorner(
clientID, sensorHandles, rayHit,
(minRange + maxRange) / 2.0): #not abs(oldDis - newDis)>1
start = time.time() # start time for distance tracking
x, y, distanceTravled = move.calcTraveldDistance(x, y, dt)
rangeData = rangeSensor.getSensorData(clientID, sensorHandles)
for i in range(
move.FRONT_SEN_START, move.FRONT_SEN_END
): # check for a pool of front sensors if there is an obstacle
if rangeData[i][3] <= 0.5:
print("FLALALALALAL")
wallOrient(clientID, sensorHandles, rayHit, True)
break
move.startMoving(clientID)
# Only check the following every 5 iterations, so that the correction of following doesn't happen too often
if counter % 5 == 0:
rangeToWallNew = rangeData[rayHit][3]
print("Current range to wall: {}".format(rangeToWallNew))
# if the distance to the wall is to little drive away from the wall
if rangeToWallNew < minRange:
move.setWheelVelocity(clientID, 0)
move.sideway(minRange - rangeToWallNew, clientID,
rightSide)
move.startMoving(clientID)
wallOrient(clientID, sensorHandles, rayHit, False)
# if the distance to the wall is too high -> drive towards the wall
elif rangeToWallNew > maxRange:
move.setWheelVelocity(clientID, 0)
move.sideway(rangeToWallNew - maxRange, clientID,
not rightSide)
move.startMoving(clientID)
wallOrient(clientID, sensorHandles, rayHit, False)
counter += 1
currentPoint = (move.getPos(clientID)[0][0],
move.getPos(clientID)[0][1])
if (
roundFinished == False and distanceTravled > 1
): #checks if the ubot drove around the obstacle (distanceTraveld > 1 is tolleranz to prevent, that the ubot thinks he finished a rond when he is at the starting point for the first time)
roundFinished = move.isSamePoint(startPoint, currentPoint)
if (move.getDistanceBetweenPoints(currentPoint, goalPoint) <
minDist): #checks for the min distance to the goal
minDist = move.getDistanceBetweenPoints(
currentPoint, goalPoint)
minPoint = currentPoint
disToMin = distanceTravled
if (
roundFinished and move.isSamePoint(minPoint, currentPoint)
and 2 * disToMin - 1 < distanceTravled
): #checks if the robot is again at the nearest point, again with toleranz
print("min Point found")
return
end = time.time() # end time for distance tracking
dt = end - start
print("drive around corner")
goAroundCorner(clientID, sensorHandles, rightSide, rayHit)
currentPoint = (move.getPos(clientID)[0][0],
move.getPos(clientID)[0][1])
if (roundFinished and move.isSamePoint(minPoint, currentPoint)):
print("min Point found")
return
def followBoundary(clientID, sensorHandles, rightSide):
print("Follow boundary: start")
# set rayHit to the ray index where the followed wall is
if rightSide:
rayHit = LEFT_RAY_NINETY
else:
rayHit = RIGHT_RAY_NINETY
targetDistanceToWall = 0.5
minRange = targetDistanceToWall - 0.05
maxRange = targetDistanceToWall + 0.05
counter = 0
minDistToTarget = -1.0
while True:
move.startMoving(clientID)
while not detectCorner(
clientID, sensorHandles, rayHit,
(minRange + maxRange) / 2.0): #not abs(oldDis - newDis)>1
rangeData = rangeSensor.getSensorData(clientID, sensorHandles)
for i in range(
move.FRONT_SEN_START, move.FRONT_SEN_END
): # check for a pool of front sensors if there is an obstacle
if rangeData[i][3] <= 0.5:
wallOrient(clientID, sensorHandles, rayHit, True)
break
move.startMoving(clientID)
# Only check the following every 5 iterations, so that the correction of following doesn't happen too often
if counter % 5 == 0:
rangeToWallNew = rangeData[rayHit][3]
print("Current range to wall: {}".format(rangeToWallNew))
# if the distance to the wall is to little drive away from the wall
if rangeToWallNew < minRange:
move.setWheelVelocity(clientID, 0)
move.sideway(minRange - rangeToWallNew, clientID,
rightSide)
move.startMoving(clientID)
wallOrient(clientID, sensorHandles, rayHit, False)
# if the distance to the wall is too high -> drive towards the wall
elif rangeToWallNew > maxRange:
move.setWheelVelocity(clientID, 0)
move.sideway(rangeToWallNew - maxRange, clientID,
not rightSide)
move.startMoving(clientID)
wallOrient(clientID, sensorHandles, rayHit, False)
counter += 1
# check the leaving conditions
freespace = calcFreeSpace(clientID, sensorHandles)
leavingCondition, minDistToTarget = calcLeavingConditin(
minDistToTarget, freespace, clientID)
if leavingCondition:
print("Leaving follow boundary ... cause: leaving condition")
print("Follow boundary: end")
return
print("drive around corner")
goAroundCorner(clientID, sensorHandles, rightSide, rayHit)
#check leaving conditions here again after going around a corner
freespace = calcFreeSpace(clientID, sensorHandles)
leavingCondition, minDistToTarget = calcLeavingConditin(
minDistToTarget, freespace, clientID)
if leavingCondition:
print("leaving cause of leaving condition")
return