distSinceObs = 0
obstacles.append(1)
else:
distSinceObs += (curSpeed) * timestep
if distSinceObs > bigGap:
bigGap = distSinceObs
obstacles.append(0)
elif not spotFound:
obstacles = []
obstacles.append(1)
if bigGap > 7 and obstacles[-1] == 1:
if (mode == 'Looking' or mode == 'Stopping') and curSpeed > 0:
accumDist += curSpeed * timestep
robot.setCruisingSpeed(0)
robot.setBrakeIntensity(1)
mode = 'Stopping'
elif curSpeed == 0 and mode == 'Stopping':
print(accumDist)
print(toTheRight)
mode = 'Parking'
errors = 0.25
spotwidth = 2.75
planleng = distSinceObs - 2 * errors
goal = [accumDist + planleng / 2, toTheRight + spotwidth / 2]
path = reeds_shepp.path_sample(
(carleng / 2, 0, 0), (goal[0], goal[1], 0), rho, step_size)
xs = np.array([q[0] for q in path])
ys = np.array([q[1] for q in path])
thetas = np.array([q[2] for q in path])
overtakingSide = None
currentLane -= 1
elif overtakingSide == 'left' and sensors["right"].getValue(
) < 0.8 * sensors["right"].getMaxValue():
overtakingSide = None
currentLane += 1
else: # reduce the speed if the vehicle from previous lane is still in front
speed2 = reduce_speed_if_vehicle_on_side(speed, overtakingSide)
if speed2 < speed:
speed = speed2
speed = get_filtered_speed(speed)
driver.setCruisingSpeed(speed)
# brake if needed
speedDiff = driver.getCurrentSpeed() - speed
if speedDiff > 0:
driver.setBrakeIntensity(min(speedDiff / speed, 1))
else:
driver.setBrakeIntensity(0)
# car in front, try to overtake
if frontDistance < 0.8 * frontRange and overtakingSide is None:
if (is_vehicle_on_side("left")
and (not safeOvertake or sensors["rear left"].getValue() >
0.8 * sensors["rear left"].getMaxValue())
and sensors["left"].getValue() >
0.8 * sensors["left"].getMaxValue() and currentLane < 2):
currentLane += 1
overtakingSide = 'right'
elif (is_vehicle_on_side("right")
and (not safeOvertake or sensors["rear right"].getValue() >
0.8 * sensors["rear right"].getMaxValue())
and sensors["right"].getValue() >
Lidar.enable(lms291, TIME_STEP)
lms291_width = Lidar.getHorizontalResolution(lms291)
print(lms291_width)
fig = plt.figure(figsize=(3, 3))
while driver.step() != -1:
if cont < 1000:
driver.setDippedBeams(True) # farol ligado
driver.setIndicator(0) # 0 -> OFF 1 -> Right 2 -> Left
driver.setCruisingSpeed(speedFoward) # acelerador (velocidade)
driver.setSteeringAngle(0.0) # volante (giro)
elif cont > 1000 and cont < 1500:
driver.setCruisingSpeed(speedBrake)
driver.setBrakeIntensity(1.0) # intensidade (0.0 a 1.0)
elif cont > 1500 and cont < 2500:
driver.setCruisingSpeed(-speedFoward) # acelerador (velocidade)
driver.setSteeringAngle(0.0) # volante (giro)
elif cont > 2500:
cont = 0
# print('speed (km/h) %0.2f' % driver.getCurrentSpeed())
cont += 1
# ler a camera
Camera.getImage(cameraRGB)
Camera.saveImage(cameraRGB, 'color.png', 1)
frameColor = cv.imread('color.png')
cv.imshow('color', frameColor)
turning_data.turning.x, turning_data.turning.y
intersection_safe, dist = turning_data.braking_info.safe, \
turning_data.braking_info.dist
break
except:
pass
if str(speed) == 'nan' or intersection_safe or ignore_smart_intersection:
continue
if not intersection_safe:
print("Human car at intersection")
if not intersection_safe:
# If very close to intersection safer to just go ahead.
intersection_safe = intersection_safe or dist < 10
if intersection_safe:
continue
print("Uber car slow down. Distance to intersection: ", dist)
# driver.setBrakeIntensity(1.0)
a = (speed**2) / (2 * dist)
if a >= 0:
b = max(min((a - friction_acc) / brake_coeff, 1), 0.001)
# b = min(a/brake_coef, 1)
else:
b = 1
if b > braking_threshold:
braking = True
if braking:
driver.setThrottle(0)
driver.setBrakeIntensity(b)
# driver.setSteeringAngle(1*0.1)
prevpos = x[0]
#continue
prevpos = x[0]
counter=counter +1
if ((speedDiff > 0)):
print(counter)
if((sensors['right'].getValue()>3)and(sensors['front right 2'].getValue()>8)and(not(sensors['right'].getValue()<7))): #you can turn right
print("trying right")
if((x[0]<5.25)and(x[0]>5.15)): #means you are left, go middle
dest = 2 #middle
if((x[0]<1.5)and(x[0]>1)): #1.3, you are middle,go right
dest = 3 #right
elif((sensors['left'].getValue()>3)and(sensors['front left 2'].getValue()>8)and(sensors['front left 1'].getValue()>14)): #You can steer left
#driver.setSteeringAngle(-speedDiff*0.1)
print("trying left")
if((x[0]<-1.5)and(x[0]>-2.5)): #-2, you are in the right row, go left
dest = 2 #middle
if((x[0]<1.5)and(x[0]>1)): #1.3, you are middle,go left
dest = 1 #left
else:
driver.setBrakeIntensity(100)
else:
driver.setBrakeIntensity(0)
maxSpeed = 120
fig = plt.figure(figsize=(3, 3))
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while driver.step() != -1:
if sayici < 1000:
driver.setCruisingSpeed(ileri_hizi) # aracın ileri doğru hızını
#belirler
driver.setSteeringAngle(0.6) #aracın + veya - durumuna
#göre sağa veya sola döndürür
driver.setDippedBeams(True) #ışığı yakma true false
driver.setIndicator(2) # aracın sinyal lambalarının sağa döndüğü 1 ile
#sola döndüğünü 2 ile kapalı tutmak için 0
elif sayici > 1000 and sayici < 1100:
driver.setCruisingSpeed(fren) #frenlerken
driver.setBrakeIntensity(1.0) # % de kaç
# frene basılsın ?
driver.setDippedBeams(False) #ışığı kapatıyoruz
elif sayici > 1100 and sayici < 1500:
driver.setCruisingSpeed(-ileri_hizi)
driver.setSteeringAngle(-0.6)
elif sayici > 1500 and sayici < 1900:
driver.setCruisingSpeed(fren) #frenlerken
driver.setBrakeIntensity(1.0) # % de kaç
# frene basılsın ?
driver.setDippedBeams(False) #ışığı kapatıyoruz
elif sayici > 1900:
sayici = 0
sayici += 1
Camera.getImage(camera)
driver.setSteeringAngle((2.6) * 0.01)
if (counter >= lim5):
maxSpeed = 100
driver.setSteeringAngle((-0.8) * 0.01)
if (counter >= lim6):
maxSpeed = 90
driver.setSteeringAngle((-0.89) * 0.01)
if (counter >= lim7):
maxSpeed = 90
driver.setSteeringAngle((3.35) * 0.01)
if (counter >= lim8):
maxSpeed = 90
driver.setSteeringAngle((-1.5) * 0.01)
if ((speedDiff > 0)):
driver.setBrakeIntensity(0.5)
else:
driver.setBrakeIntensity(0)
elif ((counter > 6700)):
if (counter > 8200):
maxSpeed = 50
if (counter == 4800):
maxSpeed = 200
frontDistance = min(sensors['front'].getValue(),
sensors['front left 0'].getValue(),
sensors['front right 0'].getValue())
frontRange = sensors['front'].getMaxValue()
speed = maxSpeed * frontDistance / frontRange
