def auto_drive_call(m_order_queue, respond_dict):
"""
Runs drive instance in the simulation.
Defining and enabling sensors.
:param respond_dict: dictionary to send value VA process
:param m_order_queue:
:type m_order_queue: multiprocessing.Queue To provide communication between voice assistant process.
:rtype None
"""
# Driver initialize
auto_drive = Driver()
auto_drive.setAntifogLights(True)
auto_drive.setDippedBeams(True)
TIME_STEP = int(auto_drive.getBasicTimeStep())
# distance sensors
dist_sensor_names = [
"front", "front right 0", "front right 1", "front right 2",
"front right 3", "front left 0", "front left 1", "front left 2",
"front left 3", "rear", "rear left", "rear right", "right", "left"
]
dist_sensors = {}
for name in dist_sensor_names:
dist_sensors[name] = auto_drive.getDistanceSensor("distance sensor " +
name)
dist_sensors[name].enable(TIME_STEP)
# GPS
gps = auto_drive.getGPS("gps")
gps.enable(TIME_STEP)
# Compass
compass = auto_drive.getCompass("compass")
compass.enable(TIME_STEP)
# get and enable front camera
front_camera1 = auto_drive.getCamera("front camera 1")
front_camera1.enable(TIME_STEP)
front_camera2 = auto_drive.getCamera("front camera 2")
front_camera2.enable(TIME_STEP)
front_camera3 = auto_drive.getCamera("front camera 3")
front_camera3.enable(TIME_STEP)
front_cams = {
"right": front_camera2,
"left": front_camera1,
"center": front_camera3
}
# get and enable back camera
back_camera = auto_drive.getCamera("camera2")
back_camera.enable(TIME_STEP * 10)
# back_camera.recognitionEnable(TIME_STEP * 10)
# Get the display devices.
# The display can be used to visually show the tracked position.
# For showing lane detection
display_front = auto_drive.getDisplay('display')
display_front.setColor(0xFF00FF)
# To establish communication between Emergency Vehicle
receiver = auto_drive.getReceiver("receiver")
receiver.enable(TIME_STEP)
# To establish communication between other vehicles
emitter = auto_drive.getEmitter("emitter")
# lidar devices
lidars = []
Log = list()
error_Log = list()
for i in range(auto_drive.getNumberOfDevices()):
device = auto_drive.getDeviceByIndex(i)
if device.getNodeType() == Node.LIDAR:
lidars.append(device)
device.enable(TIME_STEP * 10)
device.enablePointCloud()
if not lidars:
error_Log.append(" [ DRIVER CALL] This vehicle has no 'Lidar' node.")
# Set first values
auto_drive.setCruisingSpeed(40)
auto_drive.setSteeringAngle(0)
VA_order, emergency_message, prev_gps, gps_val = None, None, None, None
# Main Loop
while auto_drive.step() != -1:
start_time = time.time()
# for lidar in lidars:
# lidar.getPointCloud()
if m_order_queue.qsize() > 0:
VA_order = m_order_queue.get()
else:
VA_order = None
""" If an Emergency Vehicle in the emergency state closer than 4 metre it sends emergency message
to cars in front of it and other cars has sends messages as a chain to clear the way """
if receiver.getQueueLength() > 0:
message = receiver.getData()
# for sending emergency message to AutoCars front of our AutoCar
# emitter.send(message)
emergency_message = struct.unpack("?", message)
emergency_message = emergency_message[0]
receiver.nextPacket()
else:
emergency_message = False
gps_val = round(sqrt(gps.getValues()[0]**2 + gps.getValues()[2]**2), 2)
if gps_val is None:
error_Log.append("[DRIVER CALL] couldn't get gps value..")
else:
prev_gps = gps_val
if prev_gps is not None and gps_val is not None:
gps_val = prev_gps
if gps_val is not None:
# To calculate direction of the car
cmp_val = compass.getValues()
angle = ((atan2(cmp_val[0], cmp_val[2])) * (180 / pi)) + 180
# goes on Z axis
if 335 <= angle <= 360 or 0 <= angle <= 45 or 135 <= angle <= 225:
axis = 1
# goes on X axis
elif 225 <= angle < 335 or 45 <= angle < 135:
axis = 0
obj_data, LIDAR_data = Obj_Recognition.main(
dist_sensor_names, lidars, dist_sensors, front_cams,
back_camera)
DataFusion.main(auto_drive, gps_val, obj_data, LIDAR_data,
emergency_message, display_front, front_cams,
dist_sensors, VA_order, respond_dict, gps,
axis)
else:
error_Log.append("[DRIVER CALL] couldn't get gps value..")
Log.append(str(time.time() - start_time))
with open("Logs\Driver_Log.csv", 'a') as file:
wr = writer(file, quoting=QUOTE_ALL)
wr.writerow(Log)
if len(error_Log):
with open("Logs\error_Log.csv", 'a', newline="") as file:
wr = writer(file, quoting=QUOTE_ALL)
wr.writerow(error_Log)
# Please do not alter this file - it may cause the simulation to fail.
# Import Webots-specific functions
from controller import Display
from vehicle import Driver
# Import functions from other scripts in controller folder
from util import *
from your_controller import CustomController
from evaluation import evaluation
trajectory = getTrajectory('buggyTrace.csv')
# Instantiate supervisor and functions
driver = Driver()
driver.setDippedBeams(True)
driver.setGear(1) # Torque control mode
throttleConversion = 15737
msToKmh = 3.6
# Access and set up displays
console = driver.getDisplay("console")
speedometer = driver.getDisplay("speedometer")
console.setFont("Arial Black", 14, True)
speedometerGraphic = speedometer.imageLoad("speedometer.png")
speedometer.imagePaste(speedometerGraphic, 0, 0, True)
consoleObject = DisplayUpdate(console)
speedometerObject = DisplayUpdate(speedometer)
# Get the time step of the current world
TIME_STEP = 64 # ms
MAX_SPEED = 80 # km/h
driver = Driver()
speedFoward = 10 # km/h
speedBrake = 0 # km/h
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)
cameraRGB = driver.getCamera('camera')
Camera.enable(cameraRGB, TIME_STEP)
lms291 = driver.getLidar('Sick LMS 291')
print(lms291)
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
lms291 = driver.getLidar("Sick LMS 291")
Lidar.enable(lms291, timestep)
lms291_yatay = Lidar.getHorizontalResolution(lms291)
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
