# You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Robot, Camera, Motor, DistanceSensor import numpy as np import cv2 as cv import math import matplotlib.pyplot as plt from vehicle import Driver MAKEPLOT = False # create the Robot instance. #robot = Robot() robot = Driver() front_camera = robot.getCamera("front_camera") rear_camera = robot.getCamera("rear_camera") lidar = robot.getLidar("Sick LMS 291") #for att in dir(robot): # print(att,getattr(robot,att)) # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) print(timestep) #print(dir(robot)) # You should insert a getDevice-like function in order to get the #instance of a device of the robot. Something like: # motor = robot.getMotor('motorname') # ds = robot.getDistanceSensor('dsname') # ds.enable(timestep)
ratio = sensors[name].getValue() / sensors[name].getMaxValue() if ratio < minRatio: minRatio = ratio return minRatio * speed get_filtered_speed.previousSpeeds = [] driver = Driver() for name in sensorsNames: sensors[name] = driver.getDistanceSensor("distance sensor " + name) sensors[name].enable(10) gps = driver.getGPS("gps") gps.enable(10) camera = driver.getCamera("camera") # uncomment those lines to enable the camera camera.enable(10) camera.recognitionEnable(50) while driver.step() != -1: # adjust speed according to front vehicle frontDistance = sensors["front"].getValue() frontRange = sensors["front"].getMaxValue() speed = maxSpeed * frontDistance / frontRange if sensors["front right 0"].getValue( ) < 8.0 or sensors["front left 0"].getValue() < 8.0: # another vehicle is currently changing lane in front of the vehicle => emergency braking speed = min(0.5 * maxSpeed, speed) if overtakingSide is not None: # check if overtaking should be aborted
driver = Driver() timestep = int(driver.getBasicTimeStep()) lidar = driver.getLidar('Sick LMS 291') lidar.enable(10) accelerometer = driver.getAccelerometer("accelerometer") accelerometer.enable(timestep) # accelerometer = driver.getAccelerometer("gyro") # accelerometer.enable(timestep) # print(type(accelerometer)) front_camera = driver.getCamera("front_camera") front_camera.enable(10) #front_camera.recognitionEnable(10) back_camera = driver.getCamera("rear_camera") # rear_camera = driver.getCamera("rear_camera") # rear_camera.enable(30) CAM_WIDTH = front_camera.getWidth() CAM_HEIGHT = front_camera.getHeight() CAM_CENTER = int(CAM_WIDTH / 2) BACK_CAM_WIDTH = back_camera.getWidth() BACK_CAM_HEIGHT = back_camera.getHeight() BACK_CAM_CENTER = int(BACK_CAM_WIDTH / 2)
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)
par_dir = curr_dir[:curr_dir.rfind('/')] def LQR(v_target, wheelbase, Q, R): # print(v_target,wheelbase,Q,R) A = np.matrix([[0, v_target * (5. / 18.)], [0, 0]]) B = np.matrix([[0], [(v_target / wheelbase) * (5. / 18.)]]) V = np.matrix(linalg.solve_continuous_are(A, B, Q, R)) K = np.matrix(linalg.inv(R) * (B.T * V)) return K # create the Robot instance. driver = Driver() camera = driver.getCamera('camera') 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
import cv2 as cv import matplotlib.pyplot as plt import numpy as np import math TIME_STEP = 64 # ms MAX_SPEED = 100 # km/h driver = Driver() speedFoward = 0.1 * MAX_SPEED # km/h speedBrake = 0 # km/h cont = 0 plot = 10 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
# draw the surface enclosed by lane lines back onto the original frame #blend_on_road = draw_back_onto_the_road(img_undistorted, Minv, line_lt, line_rt, keep_state) # stitch on the top of final output images from different steps of the pipeline #blend_output = prepare_out_blend_frame(blend_on_road, img_binary, img_birdeye, img_fit, line_lt, line_rt, offset_meter) processed_frames += 1 #return blend_output return offset_meter driver = Driver() camera_lk = driver.getCamera("camera_lk") camera_lk.enable(1) #camera_lk.recognitionEnable(1) """CNN initialization""" camera_cnn = driver.getCamera("camera_cnn") camera_cnn.enable(1) #camera_cnn.recognitionEnable(1) CHECKPOINT = './data/checkpoints/model-traffic-cones.ckpt-1150' print('Loading neural net...') net = nn.init(CHECKPOINT) print('Done') speed = 10 DETECT_FREQ = 8