def globalToEgocentric(globCorrd, clientID):
pos, orient = move.getPos(clientID)
egoVec = [globCorrd[0] - pos[0], globCorrd[1] - pos[1]]
alpha = move.getOrientation(clientID) / 180.0 * math.pi + math.pi / 2.0
rotationMatrix = [[math.cos(-alpha), -math.sin(-alpha)],
[math.sin(-alpha), math.cos(-alpha)]]
newVec = np.dot(np.array(rotationMatrix), np.array(egoVec))
return newVec
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 egocentricToGlobal(ego, clientID):
x = ego[0]
y = ego[1]
pos, orient = move.getPos(clientID)
alpha = move.getOrientation(clientID) / 180.0 * math.pi + math.pi / 2.0
rotationMatrix = [[math.cos(alpha), -math.sin(alpha)],
[math.sin(alpha), math.cos(alpha)]]
newVec = np.dot(np.array(rotationMatrix), np.array([x, y]))
x = newVec[0]
y = newVec[1]
xBot = pos[0]
yBot = pos[1]
return [x + xBot, y + yBot]
def getRayToTarget(clientID):
# calculate angle to target
targetOrientation = calcOrientationToTarget(clientID, "Goal")
robotOrientation = move.getOrientation(clientID)
angleToTarget = move.substractOrientation(targetOrientation,
robotOrientation)
# choose between left and right sensor and
# calculate ray pointing to target
if angleToTarget > 0: # left sensor
ray = math.fabs(angleToTarget) / RAY_ANGLE # calculate ray as float
ray = round(ray) # round to int
ray += 342 # add first idx of left sensor
else: # right sensor
ray = (120 - math.fabs(angleToTarget)) / RAY_ANGLE
ray = round(ray)
return int(ray)
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 rotateUntilOrientation(clientID, targetOrient):
move.setWheelVelocity(clientID, 0)
# get needed youBot information's
rotationVel = 0.8
wheelJoints = move.getWheelJoints(clientID)
currentOrient = move.getOrientation(clientID)
safty = False
#print("Orientations for rotation: Current Orientation = {} , Target Orientation = {}" .format(currentOrient, targetOrient))
# The following are all possibilities that can happen when rotating until a certain orientation
if (currentOrient > 0 and targetOrient < 0
and abs(targetOrient - currentOrient) > 180):
rotationVel *= -1
move.startRotating(clientID, rotationVel)
if (targetOrient + 1) > 180:
safty = True
while not (targetOrient + 5.0 > currentOrient
and currentOrient > targetOrient - 5.0): #fails at 180/-180
time.sleep(0.05)
currentOrient = move.getOrientation(clientID)
if (currentOrient < -175 and safty == True):
break
elif (currentOrient > 0 and targetOrient < currentOrient):
rotationVel *= 1
move.startRotating(clientID, rotationVel)
if (targetOrient - 1) < -180:
safty = True
while targetOrient < currentOrient: #fails at 0
time.sleep(0.05)
currentOrient = move.getOrientation(clientID)
if (currentOrient > 175) and safty == True:
break
elif (currentOrient > 0 and targetOrient > currentOrient):
rotationVel *= -1
move.startRotating(clientID, rotationVel)
if (targetOrient + 1) > 180:
safty = True
while targetOrient > currentOrient: #fails at 180/-180
time.sleep(0.05)
currentOrient = move.getOrientation(clientID)
if (currentOrient < -175 and safty == True):
break
elif (currentOrient < 0 and targetOrient > 0
and abs(targetOrient - currentOrient) > 180):
rotationVel *= 1
move.startRotating(clientID, rotationVel)
if (targetOrient - 1) < -180:
safty = True
while not (targetOrient + 5.0 > currentOrient
and targetOrient - 5.0 < currentOrient): #fails at 0
time.sleep(0.05)
currentOrient = move.getOrientation(clientID)
if (currentOrient > 175) and safty == True:
break
elif (currentOrient < 0 and targetOrient < currentOrient):
rotationVel *= 1
move.startRotating(clientID, rotationVel)
if (targetOrient - 1) < -180:
safty = True
while targetOrient < currentOrient: #fails at 0
time.sleep(0.05)
currentOrient = move.getOrientation(clientID)
if (currentOrient > 175) and safty == True:
break
else:
rotationVel *= -1
move.startRotating(clientID, rotationVel)
if (targetOrient + 1) > 180:
safty = True
while targetOrient > currentOrient: #fails at 180/-180
time.sleep(0.05)
currentOrient = move.getOrientation(clientID)
if (currentOrient < -175 and safty == True):
break
# stop moving
move.setWheelVelocity(clientID, 0)
return