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
#!/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)
# Main loop:
# - perform simulation steps until Webots is stopping the controller
steer = 0
kp = 4.5
shadow_state = 0
# for plot
if MAKEPLOT is True:
plt.style.use('ggplot')
size = 100
x_vec = np.linspace(-20, 0, size)
y_vec = np.linspace(0, 0, size)
change = []
while robot.step() != -1:
# Read the sensors:
# Enter here functions to read sensor data, like:
# 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)
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)
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)
#grayFlip = cv2.flip(gray270, 1)
#cv2.imwrite("grayflip.jpeg", grayFlip)
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