def wander(v):
numBeams = myRobot._numberOfBeams
angle = myRobot._viewAngle
degree = angle / numBeams
while (True):
pos = -1
min = 99999
senor = myRobot.sense()
for i in range(0, numBeams):
if (senor[i] != None and senor[i] < 1):
if (min > senor[i]):
min = senor[i]
pos = i
#print(pos)
if (pos == -1):
th = 0
else:
break
th = ((pos * degree + 180 - 135 + 180) % 360) - 180
th = (th * 2 * pi) / 360
x, y, theta = myWorld.getTrueRobotPose()
wallpoints = extractSegmentsFromSensorData(
myRobot.sense(), myRobot.getSensorDirections())
myWorld.drawPolyline([
(wallpoints[0][0][0] + x, wallpoints[0][0][1] + y),
(wallpoints[1][1][0] + x, wallpoints[1][1][1] + y)
])
myRobot.move((v, th))
def findWall(self, d):
sensor = self.sense()[3:12]
for i in range(0, len(sensor)):
if sensor[i] != None and sensor[i] > d + 0.3:
sensor[i] = None
directions = self.getSensorDirections()
a = sensorUtilities.extractSegmentsFromSensorData(
sensor, directions[3:12])
return a
def control():
while True:
(motion,boxCmd,exit) = myKeyboardController.getCmd()
if exit:
break
myRobot.move(motion)
dists = myRobot.sense()
directions = myRobot.getSensorDirections()
lines_l = sensorUtilities.extractSegmentsFromSensorData(dists, directions)
lines_g = sensorUtilities.transformPolylinesL2G(lines_l, myWorld.getTrueRobotPose())
myWorld.drawPolylines(lines_g)
#print("\nsensordatenLine_l: ", min(lines_l))
#print("\nsensordatenLine_g: ", min(lines_g))
print("\ndists: ", min(dists))
# Simulation schliessen:
myWorld.close(False)
def followWall(v=0.5, d=0.75, offset=1):
directions = myRobot.getSensorDirections()[2:10]
middle_sensor = myRobot.sense()[13]
sensors = myRobot.sense()[3:9]
for i, sensor in enumerate(sensors):
if sensor != None and sensor > d + 0.3:
sensors[i] = None
wall_candidates = sensorUtilities.extractSegmentsFromSensorData(
sensors, directions)
if len(wall_candidates) > 0:
start = wall_candidates[0][0]
start[1] = start[1] + offset
end = wall_candidates[0][1]
end[1] = end[1] + offset
wall = [[start, end]]
lines = sensorUtilities.transformPolylinesL2G(
wall, myWorld.getTrueRobotPose())
myWorld.drawPolylines(lines)
if middle_sensor != None and middle_sensor < d:
myRobot.move([0, OMEGA_DEF])
else:
followLine(start, end)
else:
myRobot.move([v / 2, -OMEGA_DEF])
# Roboter in Office-World positionieren:
myWorld = officeWorld.buildWorld()
myRobot = Robot.Robot()
myWorld.setRobot(myRobot, [2, 5.5, pi / 2])
#myWorld = obstacleWorld3.buildWorld()
#myWorld.setRobot(myRobot, [1, 6, 0])
# KeyboardController definieren:
myKeyboardController = myWorld.getKeyboardController()
# Bewege Roboter mit Cursor-Tasten:
while True:
(motion, boxCmd, exit) = myKeyboardController.getCmd()
if motion == None:
break
myRobot.move(motion)
dists = myRobot.sense()
directions = myRobot.getSensorDirections()
lines_l = sensorUtilities.extractSegmentsFromSensorData(dists, directions)
lines_g = sensorUtilities.transformPolylinesL2G(lines_l,
myWorld.getTrueRobotPose())
print(lines_l)
for line in lines_l:
d, alpha = OB_LineUtilities.getDistanceAndNormAngleToLocalLine(line)
print("d, alpha", alpha * 180 / pi, d, line)
myWorld.drawPolylines(lines_g)
# Simulation schliessen:
myWorld.close(False)
def followWall(v, d):
wander(0.1)
a = 0
x, y, theta = myWorld.getTrueRobotPose()
wallpoints = extractSegmentsFromSensorData(myRobot.sense(),
myRobot.getSensorDirections())
print(wallpoints)
polarpointX1 = np.sqrt(wallpoints[0][0][0]**2 +
wallpoints[0][0][1]**2) * np.cos(
np.arctan2(wallpoints[0][0][1],
wallpoints[0][0][0]))
polarpointY1 = np.sqrt(wallpoints[0][0][0]**2 +
wallpoints[0][0][1]**2) * np.sin(
np.arctan2(wallpoints[0][0][1],
wallpoints[0][0][0]))
polarpointX2 = np.sqrt(wallpoints[0][1][0]**2 +
wallpoints[0][1][1]**2) * np.cos(
np.arctan2(wallpoints[0][1][1],
wallpoints[0][1][0]))
polarpointY2 = np.sqrt(wallpoints[0][1][0]**2 +
wallpoints[0][1][1]**2) * np.sin(
np.arctan2(wallpoints[0][1][1],
wallpoints[0][1][0]))
#print(polarpointX1, polarpointY1)
myWorld.drawPolyline([(polarpointX1 + x, polarpointY1 + y),
(polarpointX2 + x, polarpointY2 + y)])
transformtWallpoints = transformPolylinesL2G(wallpoints,
myWorld.getTrueRobotPose())
npx, npy = geometry.neareastPointOnLine((0, 0),
[(polarpointX1, polarpointY1),
(polarpointX2, polarpointY2)])
print(npx + x, npy + y)
roto = np.arctan2(npy, npx)
myRobot.move((v, roto))
r = np.sqrt(npx**2 + npy**2)
while (r > 0.9):
x, y, theta = myWorld.getTrueRobotPose()
wallpoints = extractSegmentsFromSensorData(
myRobot.sense(), myRobot.getSensorDirections())
polarpointX1 = np.sqrt(wallpoints[0][0][0]**2 +
wallpoints[0][0][1]**2) * np.cos(
np.arctan2(wallpoints[0][0][1],
wallpoints[0][0][0]))
polarpointY1 = np.sqrt(wallpoints[0][0][0]**2 +
wallpoints[0][0][1]**2) * np.sin(
np.arctan2(wallpoints[0][0][1],
wallpoints[0][0][0]))
polarpointX2 = np.sqrt(wallpoints[0][1][0]**2 +
wallpoints[0][1][1]**2) * np.cos(
np.arctan2(wallpoints[0][1][1],
wallpoints[0][1][0]))
polarpointY2 = np.sqrt(wallpoints[0][1][0]**2 +
wallpoints[0][1][1]**2) * np.sin(
np.arctan2(wallpoints[0][1][1],
wallpoints[0][1][0]))
# print(polarpointX1, polarpointY1)
myWorld.drawPolyline([(polarpointX1 + x, polarpointY1 + y),
(polarpointX2 + x, polarpointY2 + y)])
npx, npy = geometry.neareastPointOnLine((0, 0),
[(polarpointX1, polarpointY1),
(polarpointX2, polarpointY2)])
r = np.sqrt(npx**2 + npy**2)
myRobot.move((v, 0))