# 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
dt = timestep
sign = []
for i in range(len(xs) - 1):
if xs[i + 1] > xs[i]:
sign.append(1)
else:
sign.append(-1)
sign.append(-1)
robot.setBrakeIntensity(1)
else:
print(obstacles)
print(distSinceObs)
robot.setCruisingSpeed(25)
robot.setSteeringAngle(steer * np.pi / 180)
if mode == 'Parking':
ai = pid_control(sign[target_idx] * target_speed, state.v)
di, target_idx = stanley_control(state, xs, ys, thetas, target_idx)
state.update(ai, di)
time += dt
robot.setCruisingSpeed(-1 * sign[target_idx] * target_speed)
robot.setSteeringAngle(di)
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
t.append(time)
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)
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.
psidotVec = []
FVec = []
minDist = []
passMiddlePoint = False
nearGoal = False
finish = False
while driver.step() != -1:
# Call control update method
X, Y, xdot, ydot, psi, psidot, F, delta = \
customController.update(timestep)
# Set control update output
driver.setThrottle(clamp(F / throttleConversion, 0, 1))
driver.setSteeringAngle(-clamp(delta, np.radians(-30), np.radians(30)))
# Check for halfway point/completion
disError, nearIdx = closestNode(X, Y, trajectory)
consoleObject.consoleUpdate(disError, nearIdx)
speedometerObject.speedometerUpdate(speedometerGraphic, xdot * msToKmh)
stepToMiddle = nearIdx - len(trajectory) / 2.0
if abs(stepToMiddle) < 100.0 and passMiddlePoint == False:
passMiddlePoint = True
if passMiddlePoint == True:
console.drawText("Middle point passed.", 5, 60)
nearGoal = nearIdx >= len(trajectory) - 50
class Altino:
def __init__(self):
# initialize devices
self.initializeDevices()
# initial speed and steering angle values
self.speed = 0.0
self.angle = 0.0
# initial wheel length used to calculate how many m the wheels have traveled
self.leftWheelDistance = 0.0
self.rightWheelDistance = 0.0
self.updateDistanceTraveled()
# line forwared
self.lineFollower = LineFollower(self.camera)
# navigation
self.nav = PathPlanner()
self.nav.setRobotPosition(Position(4, 1))
self.nav.setGoalPosition(Position(14, 22))
self.nav.setRobotOrientation(SOUTH)
self.nav.printStatus()
# compute fastest route from robot to goal
self.turns = self.nav.getFastestRoute()
logger.debug("turns: " + str(self.turns))
# odometry
self.odometry = Odometry(self.positionSensors, self.compass,
self.nav.getRobotPosition(),
self.nav.getRobotOrientation())
# running
def run(self):
logger.info("Running..")
leftIsEmpty = False # flag used to detect left parking lot
rightIsEmpty = False # flag used to detect right parking lot
leftStartingPosition = 0.0 # length of the left parking lot
rightStartingPosition = 0.0 # length of the right parking lot
sideOfParkingLot = 0 # indicates the side of the parking lot found: -1 left, 1 right, 0 not found yet
actualTurn = 0
while self.driver.step() != -1:
## here goes code that should be executed each step ##
# get actual compass value
#compassValues = self.compass.getValues()
#logger.debug("COMPASS: "******"INIT status")
# set wheel angle
self.setAngle(0.0)
# set starting speed
self.setSpeed(0.5)
# set new status
self.setStatus(Status.TURN)
# skip to next cycle to ensure everything is working fine
continue
# FOLLOW LINE STATUS
if self.status == Status.FOLLOW_LINE:
# set cruise speed
self.setSpeed(0.5)
# compute new angle
newAngle = self.lineFollower.getNewSteeringAngle()
# logger.debug("new steering angle: " + str(newAngle))
# set new steering angle
if newAngle != UNKNOWN:
self.setAngle(newAngle)
else:
self.setStatus(Status.TURN)
# TURN STATUS
if self.status == Status.TURN:
if actualTurn < len(self.turns):
turn = self.turns[actualTurn]
self.setAngle(0.5 * turn * MAX_ANGLE)
else:
self.setStatus(Status.STOP)
# compute new angle
newAngle = self.lineFollower.getNewSteeringAngle()
# if line is found
if newAngle != UNKNOWN:
self.setStatus(Status.FOLLOW_LINE)
actualTurn += 1
continue
# FORWARD STATUS
if self.status == Status.FORWARD:
# logger.debug("FORWARD status")
# set cruise speed
self.setSpeed(0.2)
# avoing obstacles
ds = self.distanceSensors
# get distance sensors values
frontLeftSensor = ds.frontLeft.getValue()
frontRightSensor = ds.frontRight.getValue()
sideLeftSensor = ds.sideLeft.getValue()
sideRightSensor = ds.sideRight.getValue()
# set values of thresholds
tolerance = 10
sideThreshold = 950
# check if front left obstacle, turn right
if frontLeftSensor > frontRightSensor + tolerance:
self.setAngle(RIGHT * frontLeftSensor / 500.0 * MAX_ANGLE)
# logger.debug("Steering angle: " + str(RIGHT * frontLeftSensor / 1000.0 * MAX_ANGLE))
logger.debug("Steering angle: " + str(self.angle))
# check if front right obstacle, turn left
elif frontRightSensor > frontLeftSensor + tolerance:
self.setAngle(LEFT * frontRightSensor / 500.0 * MAX_ANGLE)
# logger.debug("Steering angle: " + str(LEFT * frontRightSensor / 1000.0 * MAX_ANGLE))
logger.debug("Steering angle: " + str(self.angle))
# check if side left obstacle, turn slight right
elif sideLeftSensor > sideThreshold:
self.setAngle(RIGHT * sideLeftSensor / 4000.0 * MAX_ANGLE)
# check if side right obstacle, turn slight left
elif sideRightSensor > sideThreshold:
self.setAngle(LEFT * sideRightSensor / 4000.0 * MAX_ANGLE)
# if no obstacle go straight
else:
self.setAngle(self.angle / 1.5)
# SEARCHING_PARK STATUS
if self.status == Status.SEARCHING_PARK:
# set slow speed
self.setSpeed(0.2)
# set straight wheels
self.setAngle(0.0)
# get distance and position sensors
ds = self.distanceSensors
ps = self.positionSensors
#log info for debug
logger.debug("Left Distance Sensor: " +
str(ds.sideLeft.getValue()))
logger.debug("Left Position Sensor: " +
str(ps.frontLeft.getValue()) + " rad")
logger.debug("Left Wheel Length: " +
str(self.leftWheelDistance) + " m")
logger.debug("Starting position: " +
str(leftStartingPosition) + " m")
logger.debug("Parking Lot Length: " +
str(self.leftWheelDistance -
leftStartingPosition) + " m")
sideThreshold = 650
leftSensorValue = ds.sideLeft.getValue()
rightSensorValue = ds.sideRight.getValue()
# checking parking lot on the LEFT side
if leftSensorValue < sideThreshold and not leftIsEmpty:
leftIsEmpty = True
leftStartingPosition = self.leftWheelDistance # 100
elif leftSensorValue > sideThreshold and leftIsEmpty:
leftIsEmpty = False
elif leftIsEmpty and self.leftWheelDistance - leftStartingPosition > LENGTH + LENGTH / 3:
leftStartingPosition = self.leftWheelDistance # 200 - 100
sideOfParkingLot = LEFT
self.setStatus(Status.FORWARD2)
# checking parking lot on the RIGHT side
if rightSensorValue < sideThreshold and not rightIsEmpty:
rightIsEmpty = True
rightStartingPosition = self.rightWheelDistance
elif rightSensorValue > sideThreshold and rightIsEmpty:
rightIsEmpty = False
elif rightIsEmpty and self.rightWheelDistance - rightStartingPosition > LENGTH + LENGTH / 3:
rightStartingPosition = self.rightWheelDistance
sideOfParkingLot = RIGHT
self.setStatus(Status.FORWARD2)
# this ensure that the parking manoeuvre starts after going forward and not as soon as the parking lot is detected
if self.status == Status.FORWARD2:
distance = 0.19
if sideOfParkingLot == LEFT:
if self.leftWheelDistance - leftStartingPosition > distance:
self.status = Status.PARKING
elif sideOfParkingLot == RIGHT:
if self.rightWheelDistance - rightStartingPosition > distance:
self.status = Status.PARKING
else:
logger.warning(
"Parking lot not found! I don't know if right or left."
)
self.setStatus(Status.SEARCHING_PARK)
# starting the parking manoeuvre
if self.status == Status.PARKING:
if sideOfParkingLot != LEFT and sideOfParkingLot != RIGHT:
logger.error("side of parking lot unknown.")
exit(1)
# stop the vehicle, turn the wheels and go back
self.setSpeed(0.0)
self.setAngle(sideOfParkingLot * MAX_ANGLE)
self.setSpeed(-0.1)
# when should it turn the other way
backThreshold = 400
ds = self.distanceSensors
rear = ds.back.getValue()
logger.debug("Back sensor: " + str(rear))
if rear > backThreshold:
self.status = Status.PARKING2
if self.status == Status.PARKING2:
self.setAngle(-1 * sideOfParkingLot * MAX_ANGLE)
threshold = 945
rear = self.distanceSensors.back.getValue()
if rear > threshold:
self.setStatus(Status.CENTER)
if self.status == Status.CENTER:
self.setAngle(0.0)
self.setSpeed(0.2)
rear = self.distanceSensors.back.getValue()
front = self.distanceSensors.frontCenter.getValue()
if rear - front < 20:
self.setStatus(Status.STOP)
if self.status == Status.STOP:
self.setSpeed(0.0)
self.setAngle(0.0)
# if obstacle is cleared go forward
if not self.avoidObstacle(
) and self.prevStatus == Status.PARKING2:
self.setStatus(Status.FORWARD)
if self.status == Status.MANUAL:
# get current state
speed = self.speed
angle = self.angle
# logger.debug("Current Key: " + str(currentKey))
# keyboard controlls
# accelerate
if currentKey == self.keyboard.UP:
if speed < 0:
speed += 0.02
else:
speed += 0.008
# break
elif currentKey == self.keyboard.DOWN:
if speed > 0:
speed -= 0.02
else:
speed -= 0.008
# turn left
elif currentKey == self.keyboard.LEFT:
angle -= 0.01
# turn right
elif currentKey == self.keyboard.RIGHT:
angle += 0.01
# handbreak
elif currentKey == ord(' '):
speed /= 4
# update state
self.setSpeed(speed)
self.setAngle(angle)
# initialize Altino devices
def initializeDevices(self):
# get Driver object from Webots
self.driver = Driver()
# set vehicle status
self.status = Status.INIT
self.prevStatus = Status.INIT
# get timestep
self.timestep = int(self.driver.getBasicTimeStep())
# set sensor timestep
self.sensorTimestep = 2 * self.timestep
# get lights
self.headLights = self.driver.getDevice("headlights")
self.backLights = self.driver.getDevice("backlights")
# turn on headLights
# headLights.set(1)
# get and enable camera
self.camera = self.driver.getDevice("camera")
self.camera.enable(self.sensorTimestep)
# get distance sensors
self.distanceSensors = DistanceSensors()
self.distanceSensors.frontLeft = self.driver.getDevice(
"front_left_sensor")
self.distanceSensors.frontCenter = self.driver.getDevice(
'front_center_sensor')
self.distanceSensors.frontRight = self.driver.getDevice(
'front_right_sensor')
self.distanceSensors.sideLeft = self.driver.getDevice(
'side_left_sensor')
self.distanceSensors.sideRight = self.driver.getDevice(
'side_right_sensor')
self.distanceSensors.back = self.driver.getDevice('back_sensor')
# enable distance sensors
self.distanceSensors.frontLeft.enable(self.sensorTimestep)
self.distanceSensors.frontCenter.enable(self.sensorTimestep)
self.distanceSensors.frontRight.enable(self.sensorTimestep)
self.distanceSensors.sideLeft.enable(self.sensorTimestep)
self.distanceSensors.sideRight.enable(self.sensorTimestep)
self.distanceSensors.back.enable(self.sensorTimestep)
# get position sensors
self.positionSensors = PositionSensors()
self.positionSensors.frontLeft = self.driver.getDevice(
'left_front_sensor')
self.positionSensors.frontRight = self.driver.getDevice(
'right_front_sensor')
self.positionSensors.rearLeft = self.driver.getDevice(
'left_rear_sensor')
self.positionSensors.rearRight = self.driver.getDevice(
'right_rear_sensor')
# enable position sensors
self.positionSensors.frontLeft.enable(self.sensorTimestep)
self.positionSensors.frontRight.enable(self.sensorTimestep)
self.positionSensors.rearLeft.enable(self.sensorTimestep)
self.positionSensors.rearRight.enable(self.sensorTimestep)
# get and enable compass
self.compass = self.driver.getDevice('compass')
self.compass.enable(self.sensorTimestep)
# get and enable keyboard controll
self.keyboard = self.driver.getKeyboard()
self.keyboard.enable(self.sensorTimestep)
# this ensure sensors are correctly initialized
for i in range(int(self.sensorTimestep / self.timestep) + 1):
self.driver.step()
# update cruising speed
def setSpeed(self, speed):
if (speed >= -1 * MAX_SPEED and speed <= MAX_SPEED):
self.speed = speed
elif (speed > MAX_SPEED):
self.speed = MAX_SPEED
elif (speed > -1 * MAX_SPEED):
self.speed = -1 * MAX_SPEED
self.driver.setCruisingSpeed(self.speed)
# update steering angle
def setAngle(self, angle):
# ensure angle stays between -MAX_ANGLE and MAX_ANGLE
if (angle >= -1 * MAX_ANGLE and angle <= MAX_ANGLE):
self.angle = angle
elif (angle > MAX_ANGLE):
self.angle = MAX_ANGLE
elif (angle < -1 * MAX_ANGLE):
self.angle = -1 * MAX_ANGLE
self.driver.setSteeringAngle(self.angle)
def setStatus(self, status):
# check if new status is a valid state.
if status not in list(map(int, Status)):
logger.warning("Status: " + str(status) +
" is invalid, setting STOP status.")
self.status = Status.STOP
return
# backup last status
self.prevStatus = self.status
# set new status
self.status = status
# check if vehicle is too close to an obstacle
def avoidObstacle(self):
threshold = 950
ds = self.distanceSensors
fl = ds.frontLeft.getValue()
fc = ds.frontCenter.getValue()
fr = ds.frontRight.getValue()
rear = ds.back.getValue()
if fl > threshold or fc > threshold or fr > threshold:
return True
if rear > threshold:
return True
# compute distance traveled by wheels in meters
def updateDistanceTraveled(self):
# get position sensors
ps = self.positionSensors
# get radiants from wheel
radFLW = ps.frontLeft.getValue()
radFRW = ps.frontRight.getValue()
# compute distance traveled
self.leftWheelDistance = radFLW * WHEEL_RADIUS
self.rightWheelDistance = radFRW * WHEEL_RADIUS
# logger.debug("Distance Updated - Left Wheel Length: " + str(self.leftWheelLength) + " m")
# check keyboard pressed keys and change status
def keyboardCommands(self):
# get current key
currentKey = self.keyboard.getKey()
if DEBUG and (currentKey == ord('s') or currentKey == ord('S')):
logger.debug("Current status: " + str(self.status))
# press P to find parking lot
if currentKey == ord('p') or currentKey == ord('P'):
logger.info("Looking for a parking lot")
self.setStatus(Status.SEARCHING_PARK)
# press M to manual control
elif currentKey == ord('m') or currentKey == ord('M'):
logger.info("Manual")
self.setStatus(Status.MANUAL)
# press A to auto control
elif currentKey == ord('a') or currentKey == ord('A'):
logger.info("Auto")
self.setStatus(Status.FORWARD)
# return current key to allow other controls
return currentKey
# Main loop:
# - perform simulation steps until Webots is stopping the controller
while robot.step() != -1:
# Read the sensors:
# Enter here functions to read sensor data, like:
# val = ds.getValue()
# Process sensor data here.
contour = process_camera_image(front_camera.getImage())
if contour:
pid_op = pid(np.mean(contour))
# print(pid_op)
# set_steer_angle(pid_op)
else:
robot.setSteeringAngle(0)
# Enter here functions to send actuator commands, like:
# motor.setPosition(10.0)
pass
'''
void set_steering_angle(double wheel_angle) {
// limit the difference with previous steering_angle
if (wheel_angle - steering_angle > 0.1)
wheel_angle = steering_angle + 0.1;
if (wheel_angle - steering_angle < -0.1)
wheel_angle = steering_angle - 0.1;
steering_angle = wheel_angle;
// limit range of the steering angle
if (wheel_angle > 0.5)
wheel_angle = 0.5;
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
while driver.step() != -1:
driver.setCruisingSpeed(forward_velocity)
if counter % 30 == 0:
Camera.getImage(camera)
Camera.saveImage(camera, "camera.png", 1)
img1 = cv2.imread("camera.png")
if UNET_ACTIVATE: #USING FOR ROI
# w = UNET_SHAPE[1] # frame.shape[1]
# h = UNET_SHAPE[0] # frame.shape[0]
# car_h, car_w = h, int(h / 2)
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
# ler a camera
Camera.getImage(cameraRGB)
'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')
# uncomment those lines to enable the camera
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)
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)
"""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)
qtdVermelho0 = nparrayc.shape[1]
qtdVermelho1 = nparrayc1.shape[1]
print("Quantos pixes tem no combo 0: ", qtdVermelho0)
print("Quantos pixes tem no combo 1: ", qtdVermelho1)
#erro = qtdVermelho0 - qtdVermelho1
# --- Se negativo virar para a X se positivo para Y
# --- Analisar trativa por causa do numero
#print(erro)
k = keyboard.getKey()
if k == ord('W'):
print("forward")
car.setSteeringAngle(0)
car.setCruisingSpeed(10)
elif k == ord('D'):
print("turn right")
car.setSteeringAngle(0.5)
car.setCruisingSpeed(10)
elif k == ord('A'):
print("turn left")
car.setSteeringAngle(-0.5)
car.setCruisingSpeed(10)
elif k == ord('S'):
print("stop")
car.setSteeringAngle(0)
car.setCruisingSpeed(0)
Camera.enable(camera, timestep)
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ç
# 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)
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
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(
#!/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)
"""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 = 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')
# uncomment those lines to enable the camera
camera.enable(50)
camera.recognitionEnable(50)
dest = 0
