def setUp(self):
self.driver_1 = Driver.instance()
self.driver_2 = Driver.instance()
self.agent_1 = Agent.instance()
self.agent_2 = Agent.instance()
return super().setUp()
def Main():
driver = Driver()
#supervisor = Supervisor()
TIME_STEP = int(driver.getBasicTimeStep())
start = 0
# a forever loop until client wants to exit
bot = BotController(driver)
while driver.step() != -1:
pass
"""main controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from vehicle import Driver
from controller import Camera, Lidar
import matplotlib.pyplot as plt
import cv2
import numpy as np
import math
# create the Robot instance.
driver = Driver()
# get the time step of the current world.
timestep = int(driver.getBasicTimeStep())
Max_hizi = 80
ileri_hizi = 20
fren = 0
sayici = 0
plot = 10
camera = driver.getCamera("camera")
Camera.enable(camera, timestep)
lms291 = driver.getLidar("Sick LMS 291")
Lidar.enable(lms291, timestep)
lms291_yatay = Lidar.getHorizontalResolution(lms291)
# Copyright 1996-2019 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""vehicle_driver controller."""
import math
from vehicle import Driver
driver = Driver()
driver.setSteeringAngle(0.2)
driver.setCruisingSpeed(20)
while driver.step() != -1:
angle = 0.3 * math.cos(driver.getTime())
driver.setSteeringAngle(angle)
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""vehicle_driver_altino controller."""
from vehicle import Driver
sensorMax = 1000
driver = Driver()
basicTimeStep = int(driver.getBasicTimeStep())
sensorTimeStep = 4 * basicTimeStep
front_left_sensor = driver.getDistanceSensor('front_left_sensor')
front_center_sensor = driver.getDistanceSensor('front_center_sensor')
front_right_sensor = driver.getDistanceSensor('front_right_sensor')
headlights = driver.getLED("headlights")
backlights = driver.getLED("backlights")
keyboard = driver.getKeyboard()
keyboard.enable(sensorTimeStep)
front_left_sensor.enable(sensorTimeStep)
front_center_sensor.enable(sensorTimeStep)
"""
Webots controller to manually drive a car using the arrow keys.
Authors: John Shamoon and Wisam Bunni
"""
from numpy import pi
from controller import Keyboard
from vehicle import Driver
ego_vehicle = Driver()
keyboard = Keyboard()
keyboard.enable(int(ego_vehicle.getBasicTimeStep()))
SPEED = 25
FORWARD_RIGHT = pi / 8
FORWARD_LEFT = -FORWARD_RIGHT
BACKWARD_RIGHT = -FORWARD_RIGHT
BACKWARD_LEFT = -FORWARD_LEFT
RIGHT = pi / 4
LEFT = -RIGHT
FORWARD = 0
BACKWARD = -FORWARD
NEUTRAL = 0
SIGNALS = {
(-1, -1): (NEUTRAL, NEUTRAL),
(keyboard.LEFT, -1): (NEUTRAL, LEFT),
(-1, keyboard.LEFT): (NEUTRAL, LEFT),
(keyboard.RIGHT, -1): (NEUTRAL, RIGHT),
(-1, keyboard.RIGHT): (NEUTRAL, RIGHT),
(keyboard.UP, -1): (SPEED, FORWARD),
(-1, keyboard.UP): (SPEED, FORWARD),
# Copyright 1996-2019 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""vehicle_driver controller."""
import math
from vehicle import Driver
driver = Driver()
driver.setSteeringAngle(0)
driver.setCruisingSpeed(20)
while driver.step() != -1:
driver.setSteeringAngle(0)
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)
# This file should be set as the controller for the Tesla robot node.
# 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)
# **************************************************************
# Project 09 - Disciplina de robótica Móvel UFC / IFCE / LAPISCO
# Simulação 09 com Drone Mavic 2 Pro - Webots R2020a
# Veículo BMW X5 - controles básicos
# Python 3.5 na IDE Pycharm - controller <extern>
# By: Jefferson Silva Almeida
# Data: 23/03/2020
# **************************************************************
from controller import Robot
from vehicle import Driver
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)
'front',
'front right 0',
'front right 1',
'front right 2',
'front left 0',
'front left 1',
'front left 2',
'rear',
'rear left',
'rear right',
'right',
'left']
sensors = {}
maxSpeed = 100
driver = Driver()
driver.setSteeringAngle(0.0) # go straight
# get and enable the distance sensors
for name in sensorsNames:
sensors[name] = driver.getDistanceSensor('distance sensor ' + name)
sensors[name].enable(10)
# get and enable the GPS
gps = driver.getGPS('gps')
gps.enable(10)
# get the camera
camera = driver.getCamera('camera')
"""Sample Webots controller for highway driving benchmark."""
from vehicle import Driver
# name of the available distance sensors
sensorsNames = [
'front', 'front right 0', 'front right 1', 'front right 2', 'front left 0',
'front left 1', 'front left 2', 'rear', 'rear left', 'rear right', 'right',
'left'
]
sensors = {}
maxSpeed = 80
driver = Driver()
driver.setSteeringAngle(0.0) # go straight
# get and enable the distance sensors
for name in sensorsNames:
sensors[name] = driver.getDistanceSensor('distance sensor ' + name)
sensors[name].enable(10)
# get and enable the GPS
gps = driver.getGPS('gps')
gps.enable(10)
# get the camera
camera = driver.getCamera('camera')
# uncomment those lines to enable the camera
# camera.enable(50)
# camera.recognitionEnable(50)
"""main controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot
from vehicle import Driver
Max_hizi=80.0
ileri_hizi=10.0
durma_hizi=0.0
sayici=0
# create the Robot instance.
#robot = Robot()
driver=Driver()
# get the time step of the current world.
#timestep = int(robot.getBasicTimeStep())
# 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)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while driver.step() != -1:
if sayici<1000:
#driver.setCruisingSpeed(ileri_hizi)#aracın hızı
# If siren is playing then sends emergency signal to cars in front of it in 4 metre.
if speaker.isSoundPlaying(path) or mode:
emergency_message = struct.pack("?", mode)
emitter.send(emergency_message)
if not red.get():
red.set(1)
blue.set(0)
else:
red.set(0)
blue.set(1)
if __name__ == '__main__':
# get driver instance
driver = Driver()
# set speed of the vehicle
driver.setCruisingSpeed(100)
speaker = driver.getSpeaker("Siren")
# led sensor
red = driver.getLED("red")
red.set(0)
blue = driver.getLED("blue")
blue.set(0)
# Emitter sensor to provide communication
emitter = driver.getEmitter("emitter")
path = "sounds/AmbulanceSiren.wav"
path = os.path.abspath(path)
file = os.path.isfile(path)
from vehicle import Driver
from controller import Camera, Display, Keyboard
import cv2
import numpy as np
from numpy import array
car = Driver()
# cameraFront = Camera("cameraFront")
cameraTop = Camera("cameraTop")
display = Display("displayTop")
display.attachCamera(cameraTop)
keyboard = Keyboard()
# cameraFront.enable(32)
cameraTop.enable(32)
keyboard.enable(32)
while car.step() != -1:
display.setColor(0x000000)
display.setAlpha(0.0)
display.fillRectangle(0, 0, display.getWidth(), display.getHeight())
img = cameraTop.getImage()
image = np.frombuffer(img, np.uint8).reshape(
(cameraTop.getHeight(), cameraTop.getWidth(), 4))
# cv2.imwrite("img.png", image)
gray = cv2.cvtColor(np.float32(image), cv2.COLOR_RGB2GRAY)
#--- vira a imagem da camera em 90 graus
#gray270 = np.rot90(gray, 3)
"""av_challenge_controller controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot, Camera, Display
from vehicle import Driver
import numpy as np
import cv2
# create the Robot instance.
robot = Driver()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
steering_angle = 0
# enable cameras
front_camera = robot.getCamera("front_camera")
rear_camera = robot.getCamera("rear_camera")
front_camera.enable(30)
rear_camera.enable(30)
# start engine and set cruising speed
robot.setCruisingSpeed(50)
# camera processing stuff
def process_camera_image(image):
cam_height = front_camera.getHeight()
cam_width = front_camera.getWidth()
cam_fov = front_camera.getFov()
# print("h: ", cam_height, " w: ", cam_width)
num_pixels = cam_height * cam_width
from vehicle import Driver
# Блок начальной инициализации. Вдруг кто не знает.
# name of the available distance sensors
sensorsNames = [
'front', 'front right 0', 'front right 1', 'front right 2', 'front left 0',
'front left 1', 'front left 2', 'rear', 'rear left', 'rear right', 'right',
'left'
]
sensors = {}
#выставим максимальную скорость, загрузка водителя и установка угла поворота колес в 0.0
maxSpeed = 50
driver = Driver()
driver.setSteeringAngle(0.0)
# get and enable the distance sensors
for name in sensorsNames:
sensors[name] = driver.getDistanceSensor('distance sensor ' + name)
sensors[name].enable(10)
#define values for PID
kp = 0.12 # коэффициент пропорциональной составляющей
ki = 0.0022 # коэффициент интегральной составляющей
kd = 1 # коэффициент дифференциальной составляющей
iMin = -1.0 # минимальная сумма ошибок интегральной составляющей
iMax = 1.0 # максимальная сумма ошибок интегральной составляющей
iSum = 0.0 # сумма ошибок интегральной составляющей. Изначально равна 0.
from vehicle import Driver
import os
import pickle
curr_dir = os.getcwd()
par_dir = curr_dir[:curr_dir.rfind('/')]
driver = Driver()
driver.setSteeringAngle(0.0)
driver.setGear(1)
driver.setThrottle(1)
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:
# 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 reeds_shepp
from vehicle import Driver
# 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()) / 1e3
#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)
carleng = 4.75
from vehicle import Driver
from webots.ai_controller import Controller, ControllerException
from webots.emergency_man import EmergencyManager
from webots.light_man import LightManager
driver = Driver()
timeStep = int(driver.getBasicTimeStep())
print 'hello'
ctl = Controller(driver)
lm = LightManager(driver)
em = EmergencyManager(driver)
def initialize_all():
ctl.initialize()
def run():
while driver.step() != -1:
print('cycle')
try:
if not em.halt:
# ctl.cycle()
lm.set_setting(ctl.get_light_settings())
except ControllerException as exp:
print(str(exp))
em.engage()
lm.set_setting('emergency_flash')
"""av_challenge_controller controller."""
from controller import Camera
from vehicle import Driver
import warnings
warnings.filterwarnings("ignore")
import numpy as np
DEBUG_FLAG = True
DEBUG_FLAG_OBJ = False
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")
from tensorflow.keras.models import load_model
UNET_PATH = 'C:/Users/user/Downloads/unet19-20201030T212148Z-001/unet19'
lower_bound = np.array([0, 0, 180])
upper_bound = np.array([50, 40, 255])
# UNET_PATH = 'C:/Users/user/Downloads/unet18-20201030T210359Z-001/unet18'
# lower_bound = np.array([200, 90, 180])
# upper_bound = np.array([255, 180, 255])
model = load_model(UNET_PATH)
UNET_SHAPE = (256, 256, 3)
pre = preprocess()
UNET_ACTIVATE = True
driver = Driver()
timestep = int(driver.getBasicTimeStep())
forward_velocity = 20
brake = 0
camera = driver.getCamera("camera")
Camera.enable(camera, timestep)
# lms291 = driver.getLidar("Sick LMS 291")
# Lidar.enable(lms291, timestep)
# lms291_yatay = Lidar.getHorizontalResolution(lms291)
degree = 0
driver.setSteeringAngle(degree)
counter = 0
#Set MAKEPLOT = True to see real time plotting of vehicle speed
# 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')
# Data: 24/03/2020
# **************************************************************
import cv2
import numpy as np
import argparse
import time
from vehicle import Driver
from controller import Camera
import math
TIME_STEP = 32 # 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)
# Load yolo
def load_yolo():
net = cv2.dnn.readNet("yolov3.weights", "yolov3.cfg")
classes = []
with open("coco.names", "r") as f:
"""main controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from vehicle import Driver
from controller import Camera, Lidar
import matplotlib.pyplot as plt
import cv2
import numpy as np
import math
# create the Robot instance.
driver = Driver()
# get the time step of the current world.
timestep = int(driver.getBasicTimeStep())
Max_hizi = 80
ileri_hizi = 20
fren = 0
sayici = 0
plot = 10
camera = driver.getCamera("camera")
Camera.enable(camera, timestep)
lms291 = driver.getLidar("Sick LMS 291")
Lidar.enable(lms291, timestep)
lms291_yatay = Lidar.getHorizontalResolution(lms291)
sys.path.append("../../realsysalgorithm")
from FIFO import FIFO
from EDF import EDF
from Priority import Priority
from RoundRobin import RoundRobin
from task import Task
display = None
collision_avoidance = None
gps = None
camera = None
speedometer_image = None
autodrive = True
driver = Driver()
basicTimeStep = int(driver.getBasicTimeStep())
TIME_STEP = 100
UNKNOWN = 99999.9
FILTER_SIZE = 3
steering_angle = 0.0
KP = 0.25
KI = 0.00
KD = 2
gps_coords = [0.0 for i in range(3)]
gps_speed = 0.0
speed = 0
sick_fov = -1.0
keyboard = Keyboard()
return False
def reduce_speed_if_vehicle_on_side(speed, side):
"""Reduce the speed if there is some vehicle on the side given in argument."""
minRatio = 1
for i in range(3):
name = "front " + overtakingSide + " " + str(i)
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()
# By: Jefferson Silva Almeida
# Data: 24/03/2020
# **************************************************************
from vehicle import Driver
from controller import Camera
from controller import Lidar
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)
#!/usr/bin/env python
"""vehicle_driver controller."""
import rospy
import math
from nav_msgs.msg import Odometry
from vehicle import Driver
#from cars_stage1.msg import ControlPoints, VehicleInfo, VehicleInfoArray,Location,CarDimensions,CarVelocity
from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
rospy.init_node('vehicle_driver', anonymous=True)
pub = rospy.Publisher('/ego_vehicle', Odometry, queue_size=20)
rate = rospy.Rate(10)
odom = Odometry()
driver = Driver()
driver.setSteeringAngle(0)
driver.setCruisingSpeed(20)
gps = driver.getGPS("gps")
gps.enable(1)
while driver.step() != -1:
driver.setSteeringAngle(0)
print(gps.getValues()[0], gps.getValues()[2])
x = gps.getValues()[0]
y = gps.getValues()[2]
odom.pose.pose = Pose(Point(x, y, 0), Quaternion(0, 0, 0, 0))
print(gps.getSpeed())
vel = gps.getSpeed()
odom.twist.twist = Twist(Vector3(vel, 0, 0), Vector3(0, 0, 0))
pub.publish(odom)
curr_dir = os.getcwd()
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)