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')
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, Motor, DistanceSensor
import numpy as np
import cv2 as cv
import math
import matplotlib.pyplot as plt
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")
MAKEPLOT = False # TODO next need to add plotting for things other than speed
#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')
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()
