# val = ds.getValue()
img_f = front_camera.getImage()
img_cv = np.frombuffer(img_f, np.uint8).reshape((front_camera.getHeight(), front_camera.getWidth(), 4))
#front_camera.saveImage("stop.png",100)
nav_con = process_front(img_cv,shadow_state)
weight = nav_con[0]
shadow_state = nav_con[1]
steer = kp*(weight/25000)
if(abs(weight)>500):
robot.setCruisingSpeed(35)
else:
robot.setCruisingSpeed(50)
if(shadow_state==1):
robot.setCruisingSpeed(25)
print(weight)
print(steer)
robot.setSteeringAngle(steer)
# for plot
if MAKEPLOT is True:
rand_val = np.random.randn(1)
y_vec[-1] = float(robot.getCurrentSpeed())
change = live_plotter(x_vec,y_vec,change, "Webots Tesla Racecourse Simulation Speed")
y_vec = np.append(y_vec[1:],0.0)
pass
if overtakingSide == 'right' and sensors["left"].getValue(
) < 0.8 * sensors["left"].getMaxValue():
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() >
# Main loop:
# - perform simulation steps until Webots is stopping the controller
rho = 5.8
step_size = 0.01
steer = 0
distSinceObs = 0
bigGap = 0
accumDist = 0
obstacles = []
obstacles.append(0)
spotFound = False
mode = 'Looking'
oldSpeed = 0
while robot.step() != -1:
curSpeed = robot.getCurrentSpeed() * 1000 / 3600
rangeData = lidar.getRangeImage()
toTheRight = rangeData[-1]
if 25 * .99 < (robot.getCurrentSpeed()) < 25 * 1.0:
if toTheRight < 10:
distSinceObs = 0
obstacles.append(1)
elif toTheRight < 70 and obstacles[-1] == 1:
distSinceObs = 0
obstacles.append(1)
else:
distSinceObs += (curSpeed) * timestep
if distSinceObs > bigGap:
bigGap = distSinceObs
obstacles.append(0)
:param delta: (float) Steering
"""
# delta = np.clip(delta, -max_steer, max_steer)
self.v = curV
self.x += self.v * np.cos(self.yaw) * dt
self.y += self.v * np.sin(self.yaw) * dt
self.yaw += self.v / L * np.tan(-delta) * dt
while robot.step() != -1:
print('Current speed: {}'.format(curSpeed))
rangeData = lidar.getRangeImage()
toTheRight = rangeData[-1]
if 25 * .99 < (robot.getCurrentSpeed()) < 25 * 1.0 and mode == 'Looking':
if toTheRight < 10:
distSinceObs = 0
obstacles.append(1)
elif toTheRight < 70 and obstacles[-1] == 1:
distSinceObs = 0
obstacles.append(1)
else:
distSinceObs += (curSpeed) * timestep
if distSinceObs > bigGap:
bigGap = distSinceObs
obstacles.append(0)
elif not spotFound:
obstacles = []
obstacles.append(1)
camera.enable(1)
init_speed = 1 # must be bigger than 0
speed = init_speed
wheelbase = 2.8
cruising_speed = 30
car_length = 3.0
driver.setSteeringAngle(0.0)
driver.setCruisingSpeed(30)
while driver.step() != -1:
driver.setCruisingSpeed(30)
# get speed
speed = max(init_speed, driver.getCurrentSpeed())
# get the LQR feedback law for current speed
K = LQR(speed, wheelbase, np.array([[1, 0], [0, 3]]), np.array([[50]]))
# get data from the supervisor
while True:
try:
with open(
par_dir +
'/scenic_intersection_supervisor/data_turning.pickle',
"rb") as f:
turning_data = pickle.load(f)
th_s, x_s, y_s = -turning_data.turning.theta, \
turning_data.turning.x, turning_data.turning.y
x1, y1, x2, y2 = turning_data.line.x1, turning_data.line.y1, \
driver.setCruisingSpeed(100)
camera = driver.getCamera('camera')
camera.enable(1)
brake_coeff = 9.0
counter = 0
braking = False
braking_threshold = 0.3
friction_acc = 0.5
ignore_smart_intersection = True # Set this to True if you want to ignore smart intersection
while driver.step() != -1:
counter = counter + 1
speed = driver.getCurrentSpeed() * (5. / 18.)
while True:
try:
with open(
par_dir +
'/scenic_intersection_supervisor/data_ego.pickle',
"rb") as f:
turning_data = pickle.load(f)
th_s, x_s, y_s = -turning_data.turning.theta, \
turning_data.turning.x, turning_data.turning.y
intersection_safe, dist = turning_data.braking_info.safe, \
turning_data.braking_info.dist
break
except:
pass
cont = 0
while driver.step() != -1:
if cont < 1000:
driver.setCruisingSpeed(speedFoward) # acelerador (velocidade)
driver.setSteeringAngle(-0.7) # volante (giro)
# print('speed up %d' % cont)
driver.setDippedBeams(True) # farol ligado
driver.setIndicator(2) # 0 -> OFF 1 -> Right 2 -> Left
elif cont > 1000 and cont < 1100:
driver.setCruisingSpeed(speedBrake)
driver.setBrakeIntensity(1.0) # intensidade (0.0 a 1.0)
driver.setDippedBeams(False) # farol apagado
# print('braked %d' % cont)
elif cont > 1100 and cont < 1400:
driver.setCruisingSpeed(-speedFoward)
driver.setSteeringAngle(-0.7)
# print('speed up %d' % cont)
elif cont > 1400 and cont < 1500:
driver.setCruisingSpeed(speedBrake)
driver.setBrakeIntensity(1.0)
driver.setDippedBeams(False) # farol apagado
# print('braked %d' % cont)
elif cont > 1500:
cont = 0
print('speed (km/h) %0.2f' % driver.getCurrentSpeed())
cont += 1
