diff --git a/Dockerfile b/Dockerfile deleted file mode 100644 index b49b254..0000000 --- a/Dockerfile +++ /dev/null @@ -1,54 +0,0 @@ -FROM nvidia/vulkan:1.1.121-cuda-10.1--ubuntu18.04 -ENV DEBIAN_FRONTEND=noninteractive -ENV XDG_RUNTIME_DIR=/tmp/runtime-dir -ARG VERSION - -# Add CUDA repository key and install packages -RUN apt-key adv --fetch-keys "https://developer.download.nvidia.com/compute/cuda/repos/ubuntu1804/x86_64/3bf863cc.pub" \ - && apt update \ - && apt install -y --no-install-recommends \ - nano \ - vim \ - sudo \ - xvfb \ - ffmpeg \ - curl \ - unzip \ - libvulkan1 \ - libc++1 \ - libc++abi1 \ - && rm -rf /var/lib/apt/lists/* - -# Install AutoDRIVE Simulator app -RUN cd /home && \ - if [ -z ${VERSION+x} ]; then \ - curl -SL -o AutoDRIVE_Simulator.zip https://github.com/Tinker-Twins/AutoDRIVE/releases/download/Simulator-0.3.0/AutoDRIVE_Simulator_Linux.zip; \ - unzip AutoDRIVE_Simulator.zip -d . && \ - rm AutoDRIVE_Simulator.zip && \ - mv AutoDRIVE* AutoDRIVE_Simulator; \ - elif [ "$VERSION" = "local" ]; then \ - echo "Using local AutoDRIVE_Simulator"; \ - else \ - curl -SL -o AutoDRIVE_Simulator.zip https://github.com/Tinker-Twins/AutoDRIVE/releases/download/${VERSION}/AutoDRIVE_Simulator_Linux.zip; \ - unzip AutoDRIVE_Simulator.zip -d . && \ - rm AutoDRIVE_Simulator.zip && \ - mv AutoDRIVE* AutoDRIVE_Simulator; \ - fi - -# If VERSION is set to "local", copy from the local path to the Docker image -# Adjust first path and folder name accordingly - -### -#COPY AutoDRIVE_Simulator /home/AutoDRIVE_Simulator -### - -# Note: The above COPY instruction should be uncommented if you want to copy -# from a local path into the Docker image. - -COPY simulation_stream_recorder.sh /home/AutoDRIVE_Simulator/simulation_stream_recorder.sh -RUN mkdir /home/AutoDRIVE_Simulator/output - -WORKDIR /home/AutoDRIVE_Simulator -RUN chmod +x /home/AutoDRIVE_Simulator/AutoDRIVE\ Simulator.x86_64 -RUN chmod +x /home/AutoDRIVE_Simulator/simulation_stream_recorder.sh - diff --git a/autodrive.py b/autodrive.py new file mode 100644 index 0000000..0c4bff9 --- /dev/null +++ b/autodrive.py @@ -0,0 +1,772 @@ +#!/usr/bin/env python + +# Import libraries +import numpy as np +import base64 +from io import BytesIO +from PIL import Image +import cv2 + +################################################################################ + +# Nigel class +class Nigel: + def __init__(self): + # Nigel data + self.id = None + self.throttle = None + self.steering = None + self.encoder_ticks = None + self.encoder_angles = None + self.position = None + self.orientation_quaternion = None + self.orientation_euler_angles = None + self.angular_velocity = None + self.linear_acceleration = None + self.lidar_scan_rate = None + self.lidar_range_array = None + self.lidar_intensity_array = None + self.front_camera_image = None + self.rear_camera_image = None + # Nigel commands + self.cosim_mode = None + self.posX_command = None + self.posY_command = None + self.posZ_command = None + self.rotX_command = None + self.rotY_command = None + self.rotZ_command = None + self.rotW_command = None + self.throttle_command = None + self.steering_command = None + self.headlights_command = None + self.indicators_command = None + + # Parse Nigel sensor data + def parse_data(self, data, verbose=False): + # Actuator feedbacks + self.throttle = float(data[self.id + " Throttle"]) + self.steering = float(data[self.id + " Steering"]) + # Wheel encoders + self.encoder_ticks = np.fromstring(data[self.id + " Encoder Ticks"], dtype=int, sep=' ') + self.encoder_angles = np.fromstring(data[self.id + " Encoder Angles"], dtype=float, sep=' ') + # IPS + self.position = np.fromstring(data[self.id + " Position"], dtype=float, sep=' ') + # IMU + self.orientation_quaternion = np.fromstring(data[self.id + " Orientation Quaternion"], dtype=float, sep=' ') + self.orientation_euler_angles = np.fromstring(data[self.id + " Orientation Euler Angles"], dtype=float, sep=' ') + self.angular_velocity = np.fromstring(data[self.id + " Angular Velocity"], dtype=float, sep=' ') + self.linear_acceleration = np.fromstring(data[self.id + " Linear Acceleration"], dtype=float, sep=' ') + # LIDAR + self.lidar_scan_rate = float(data[self.id + " LIDAR Scan Rate"]) + self.lidar_range_array = np.fromstring(data[self.id + " LIDAR Range Array"], dtype=float, sep=' ') + self.lidar_intensity_array = np.fromstring(data[self.id + " LIDAR Intensity Array"], dtype=float, sep=' ') + # Cameras + self.front_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Front Camera Image"])))), cv2.COLOR_RGB2BGR) + self.rear_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Rear Camera Image"])))), cv2.COLOR_RGB2BGR) + if verbose: + print('\n--------------------------------') + print('Receive Data from Nigel: ' + self.id) + print('--------------------------------\n') + # Monitor Nigel data + print('Throttle: {}'.format(self.throttle)) + print('Steering: {}'.format(self.steering)) + print('Encoder Ticks: {} {}'.format(self.encoder_ticks[0],self.encoder_ticks[1])) + print('Encoder Angles: {} {}'.format(self.encoder_angles[0],self.encoder_angles[1])) + print('Position: {} {} {}'.format(self.position[0],self.position[1],self.position[2])) + print('Orientation [Quaternion]: {} {} {} {}'.format(self.orientation_quaternion[0],self.orientation_quaternion[1],self.orientation_quaternion[2],self.orientation_quaternion[3])) + print('Orientation [Euler Angles]: {} {} {}'.format(self.orientation_euler_angles[0],self.orientation_euler_angles[1],self.orientation_euler_angles[2])) + print('Angular Velocity: {} {} {}'.format(self.angular_velocity[0],self.angular_velocity[1],self.angular_velocity[2])) + print('Linear Acceleration: {} {} {}'.format(self.linear_acceleration[0],self.linear_acceleration[1],self.linear_acceleration[2])) + print('LIDAR Scan Rate: {}'.format(self.lidar_scan_rate)) + print('LIDAR Range Array: \n{}'.format(self.lidar_range_array)) + print('LIDAR Intensity Array: \n{}'.format(self.lidar_intensity_array)) + cv2.imshow(self.id + ' Front Camera Preview', cv2.resize(self.front_camera_image, (640, 360))) + cv2.imshow(self.id + ' Rear Camera Preview', cv2.resize(self.rear_camera_image, (640, 360))) + cv2.waitKey(1) + + # Generate Nigel control commands + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------') + print('Transmit Data to Nigel: ' + self.id) + print('-------------------------------\n') + # Monitor Nigel control commands + if self.cosim_mode == 0: + cosim_mode_str = 'False' + else: + cosim_mode_str = 'True' + print('Co-Simulation Mode: {}'.format(cosim_mode_str)) + print('Position Command: X: {} Y: {} Z: {}'.format(self.posX_command, self.posY_command, self.posZ_command)) + print('Rotation Command: X: {} Y: {} Z: {} Z: {}'.format(self.rotX_command, self.rotY_command, self.rotZ_command, self.rotW_command)) + print('Throttle Command: {}'.format(self.throttle_command)) + print('Steering Command: {}'.format(self.steering_command)) + if self.headlights_command == 0: + headlights_cmd_str = 'Disabled' + elif self.headlights_command == 1: + headlights_cmd_str = 'Low Beam' + elif self.headlights_command == 2: + headlights_cmd_str = 'High Beam' + else: + headlights_cmd_str = 'Invalid' + print('Headlights Command: {}'.format(headlights_cmd_str)) + if self.indicators_command == 0: + indicators_cmd_str = 'Disabled' + elif self.indicators_command == 1: + indicators_cmd_str = 'Left Turn Indicator' + elif self.indicators_command == 2: + indicators_cmd_str = 'Right Turn Indicator' + elif self.indicators_command == 3: + indicators_cmd_str = 'Hazard Indicator' + else: + indicators_cmd_str = 'Invalid' + print('Indicators Command: {}'.format(indicators_cmd_str)) + return {str(self.id) + ' CoSim': str(self.cosim_mode), + str(self.id) + ' PosX': str(self.posX_command), str(self.id) + ' PosY': str(self.posY_command), str(self.id) + ' PosZ': str(self.posZ_command), + str(self.id) + ' RotX': str(self.rotX_command), str(self.id) + ' RotY': str(self.rotY_command), str(self.id) + ' RotZ': str(self.rotZ_command), str(self.id) + ' RotW': str(self.rotW_command), + str(self.id) + ' Throttle': str(self.throttle_command), str(self.id) + ' Steering': str(self.steering_command), + str(self.id) + ' Headlights': str(self.headlights_command), str(self.id) + ' Indicators': str(self.indicators_command)} + +################################################################################ + +# F1TENTH class +class F1TENTH: + def __init__(self): + # F1TENTH data + self.id = None + self.throttle = None + self.steering = None + self.encoder_ticks = None + self.encoder_angles = None + self.position = None + self.orientation_quaternion = None + self.orientation_euler_angles = None + self.angular_velocity = None + self.linear_acceleration = None + self.lidar_scan_rate = None + self.lidar_range_array = None + self.lidar_intensity_array = None + self.front_camera_image = None + # F1TENTH commands + self.cosim_mode = None + self.posX_command = None + self.posY_command = None + self.posZ_command = None + self.rotX_command = None + self.rotY_command = None + self.rotZ_command = None + self.rotW_command = None + self.throttle_command = None + self.steering_command = None + + # Parse F1TENTH sensor data + def parse_data(self, data, verbose=False): + # Actuator feedbacks + self.throttle = float(data[self.id + " Throttle"]) + self.steering = float(data[self.id + " Steering"]) + # Wheel encoders + self.encoder_ticks = np.fromstring(data[self.id + " Encoder Ticks"], dtype=int, sep=' ') + self.encoder_angles = np.fromstring(data[self.id + " Encoder Angles"], dtype=float, sep=' ') + # IPS + self.position = np.fromstring(data[self.id + " Position"], dtype=float, sep=' ') + # IMU + self.orientation_quaternion = np.fromstring(data[self.id + " Orientation Quaternion"], dtype=float, sep=' ') + self.orientation_euler_angles = np.fromstring(data[self.id + " Orientation Euler Angles"], dtype=float, sep=' ') + self.angular_velocity = np.fromstring(data[self.id + " Angular Velocity"], dtype=float, sep=' ') + self.linear_acceleration = np.fromstring(data[self.id + " Linear Acceleration"], dtype=float, sep=' ') + # LIDAR + self.lidar_scan_rate = float(data[self.id + " LIDAR Scan Rate"]) + self.lidar_range_array = np.fromstring(data[self.id + " LIDAR Range Array"], dtype=float, sep=' ') + self.lidar_intensity_array = np.fromstring(data[self.id + " LIDAR Intensity Array"], dtype=float, sep=' ') + # Cameras + self.front_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Front Camera Image"])))), cv2.COLOR_RGB2BGR) + if verbose: + print('\n--------------------------------') + print('Receive Data from F1TENTH: ' + self.id) + print('--------------------------------\n') + # Monitor F1TENTH data + print('Throttle: {}'.format(self.throttle)) + print('Steering: {}'.format(self.steering)) + print('Encoder Ticks: {} {}'.format(self.encoder_ticks[0],self.encoder_ticks[1])) + print('Encoder Angles: {} {}'.format(self.encoder_angles[0],self.encoder_angles[1])) + print('Position: {} {} {}'.format(self.position[0],self.position[1],self.position[2])) + print('Orientation [Quaternion]: {} {} {} {}'.format(self.orientation_quaternion[0],self.orientation_quaternion[1],self.orientation_quaternion[2],self.orientation_quaternion[3])) + print('Orientation [Euler Angles]: {} {} {}'.format(self.orientation_euler_angles[0],self.orientation_euler_angles[1],self.orientation_euler_angles[2])) + print('Angular Velocity: {} {} {}'.format(self.angular_velocity[0],self.angular_velocity[1],self.angular_velocity[2])) + print('Linear Acceleration: {} {} {}'.format(self.linear_acceleration[0],self.linear_acceleration[1],self.linear_acceleration[2])) + print('LIDAR Scan Rate: {}'.format(self.lidar_scan_rate)) + print('LIDAR Range Array: \n{}'.format(self.lidar_range_array)) + print('LIDAR Intensity Array: \n{}'.format(self.lidar_intensity_array)) + cv2.imshow(self.id + ' Front Camera Preview', cv2.resize(self.front_camera_image, (640, 360))) + cv2.waitKey(1) + + # Generate F1TENTH control commands + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------') + print('Transmit Data to F1TENTH: ' + self.id) + print('-------------------------------\n') + # Monitor F1TENTH control commands + if self.cosim_mode == 0: + cosim_mode_str = 'False' + else: + cosim_mode_str = 'True' + print('Co-Simulation Mode: {}'.format(cosim_mode_str)) + print('Position Command: X: {} Y: {} Z: {}'.format(self.posX_command, self.posY_command, self.posZ_command)) + print('Rotation Command: X: {} Y: {} Z: {} Z: {}'.format(self.rotX_command, self.rotY_command, self.rotZ_command, self.rotW_command)) + print('Throttle Command: {}'.format(self.throttle_command)) + print('Steering Command: {}'.format(self.steering_command)) + return {str(self.id) + ' CoSim': str(self.cosim_mode), + str(self.id) + ' PosX': str(self.posX_command), str(self.id) + ' PosY': str(self.posY_command), str(self.id) + ' PosZ': str(self.posZ_command), + str(self.id) + ' RotX': str(self.rotX_command), str(self.id) + ' RotY': str(self.rotY_command), str(self.id) + ' RotZ': str(self.rotZ_command), str(self.id) + ' RotW': str(self.rotW_command), + str(self.id) + ' Throttle': str(self.throttle_command), str(self.id) + ' Steering': str(self.steering_command)} + +################################################################################ + +# Husky class +class Husky: + def __init__(self): + # Husky data + self.id = None + self.throttle = None + self.steering = None + self.encoder_ticks = None + self.encoder_angles = None + self.position = None + self.orientation_quaternion = None + self.orientation_euler_angles = None + self.angular_velocity = None + self.linear_acceleration = None + self.lidar_pointcloud = None + self.front_camera_image = None + self.rear_camera_image = None + # Husky commands + self.cosim_mode = None + self.posX_command = None + self.posY_command = None + self.posZ_command = None + self.rotX_command = None + self.rotY_command = None + self.rotZ_command = None + self.rotW_command = None + self.throttle_command = None + self.steering_command = None + + # Parse Husky sensor data + def parse_data(self, data, verbose=False): + # Actuator feedbacks + self.throttle = float(data[self.id + " Throttle"]) + self.steering = float(data[self.id + " Steering"]) + # Wheel encoders + self.encoder_ticks = np.fromstring(data[self.id + " Encoder Ticks"], dtype=int, sep=' ') + self.encoder_angles = np.fromstring(data[self.id + " Encoder Angles"], dtype=float, sep=' ') + # IPS + self.position = np.fromstring(data[self.id + " Position"], dtype=float, sep=' ') + # IMU + self.orientation_quaternion = np.fromstring(data[self.id + " Orientation Quaternion"], dtype=float, sep=' ') + self.orientation_euler_angles = np.fromstring(data[self.id + " Orientation Euler Angles"], dtype=float, sep=' ') + self.angular_velocity = np.fromstring(data[self.id + " Angular Velocity"], dtype=float, sep=' ') + self.linear_acceleration = np.fromstring(data[self.id + " Linear Acceleration"], dtype=float, sep=' ') + # LIDAR + self.lidar_pointcloud = np.fromstring(data[self.id + " LIDAR Pointcloud"], dtype=np.uint8, sep=' ') + # Cameras + self.front_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Front Camera Image"])))), cv2.COLOR_RGB2BGR) + self.rear_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Rear Camera Image"])))), cv2.COLOR_RGB2BGR) + if verbose: + print('\n--------------------------------') + print('Receive Data from Husky: ' + self.id) + print('--------------------------------\n') + # Monitor Husky data + print('Throttle: {}'.format(self.throttle)) + print('Steering: {}'.format(self.steering)) + print('Encoder Ticks: {} {}'.format(self.encoder_ticks[0],self.encoder_ticks[1])) + print('Encoder Angles: {} {}'.format(self.encoder_angles[0],self.encoder_angles[1])) + print('Position: {} {} {}'.format(self.position[0],self.position[1],self.position[2])) + print('Orientation [Quaternion]: {} {} {} {}'.format(self.orientation_quaternion[0],self.orientation_quaternion[1],self.orientation_quaternion[2],self.orientation_quaternion[3])) + print('Orientation [Euler Angles]: {} {} {}'.format(self.orientation_euler_angles[0],self.orientation_euler_angles[1],self.orientation_euler_angles[2])) + print('Angular Velocity: {} {} {}'.format(self.angular_velocity[0],self.angular_velocity[1],self.angular_velocity[2])) + print('Linear Acceleration: {} {} {}'.format(self.linear_acceleration[0],self.linear_acceleration[1],self.linear_acceleration[2])) + print('LIDAR Pointcloud: \n{}'.format(self.lidar_pointcloud)) + cv2.imshow(self.id + ' Front Camera Preview', cv2.resize(self.front_camera_image, (640, 360))) + cv2.imshow(self.id + ' Rear Camera Preview', cv2.resize(self.rear_camera_image, (640, 360))) + cv2.waitKey(1) + + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------') + print('Transmit Data to Husky: ' + self.id) + print('-------------------------------\n') + # Monitor Husky control commands + if self.cosim_mode == 0: + cosim_mode_str = 'False' + else: + cosim_mode_str = 'True' + print('Co-Simulation Mode: {}'.format(cosim_mode_str)) + print('Position Command: X: {} Y: {} Z: {}'.format(self.posX_command, self.posY_command, self.posZ_command)) + print('Rotation Command: X: {} Y: {} Z: {} Z: {}'.format(self.rotX_command, self.rotY_command, self.rotZ_command, self.rotW_command)) + print('Throttle Command: {}'.format(self.throttle_command)) + print('Steering Command: {}'.format(self.steering_command)) + return {str(self.id) + ' CoSim': str(self.cosim_mode), + str(self.id) + ' PosX': str(self.posX_command), str(self.id) + ' PosY': str(self.posY_command), str(self.id) + ' PosZ': str(self.posZ_command), + str(self.id) + ' RotX': str(self.rotX_command), str(self.id) + ' RotY': str(self.rotY_command), str(self.id) + ' RotZ': str(self.rotZ_command), str(self.id) + ' RotW': str(self.rotW_command), + str(self.id) + ' Throttle': str(self.throttle_command), str(self.id) + ' Steering': str(self.steering_command)} + +################################################################################ + +# Hunter SE class +class HunterSE: + def __init__(self): + # Hunter SE data + self.id = None + self.throttle = None + self.steering = None + self.encoder_ticks = None + self.encoder_angles = None + self.position = None + self.orientation_quaternion = None + self.orientation_euler_angles = None + self.angular_velocity = None + self.linear_acceleration = None + self.lidar_pointcloud = None + self.front_camera_image = None + self.rear_camera_image = None + # Hunter SE commands + self.cosim_mode = None + self.posX_command = None + self.posY_command = None + self.posZ_command = None + self.rotX_command = None + self.rotY_command = None + self.rotZ_command = None + self.rotW_command = None + self.throttle_command = None + self.steering_command = None + + # Parse Hunter SE sensor data + def parse_data(self, data, verbose=False): + # Actuator feedbacks + self.throttle = float(data[self.id + " Throttle"]) + self.steering = float(data[self.id + " Steering"]) + # Wheel encoders + self.encoder_ticks = np.fromstring(data[self.id + " Encoder Ticks"], dtype=int, sep=' ') + self.encoder_angles = np.fromstring(data[self.id + " Encoder Angles"], dtype=float, sep=' ') + # IPS + self.position = np.fromstring(data[self.id + " Position"], dtype=float, sep=' ') + # IMU + self.orientation_quaternion = np.fromstring(data[self.id + " Orientation Quaternion"], dtype=float, sep=' ') + self.orientation_euler_angles = np.fromstring(data[self.id + " Orientation Euler Angles"], dtype=float, sep=' ') + self.angular_velocity = np.fromstring(data[self.id + " Angular Velocity"], dtype=float, sep=' ') + self.linear_acceleration = np.fromstring(data[self.id + " Linear Acceleration"], dtype=float, sep=' ') + # LIDAR + self.lidar_pointcloud = np.frombuffer(base64.b64decode(data[self.id + " LIDAR Pointcloud"]), dtype=np.uint8) + # Cameras + self.front_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Front Camera Image"])))), cv2.COLOR_RGB2BGR) + self.rear_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Rear Camera Image"])))), cv2.COLOR_RGB2BGR) + if verbose: + print('\n--------------------------------') + print('Receive Data from Hunter SE: ' + self.id) + print('--------------------------------\n') + # Monitor Hunter SE data + print('Throttle: {}'.format(self.throttle)) + print('Steering: {}'.format(self.steering)) + print('Encoder Ticks: {} {}'.format(self.encoder_ticks[0],self.encoder_ticks[1])) + print('Encoder Angles: {} {}'.format(self.encoder_angles[0],self.encoder_angles[1])) + print('Position: {} {} {}'.format(self.position[0],self.position[1],self.position[2])) + print('Orientation [Quaternion]: {} {} {} {}'.format(self.orientation_quaternion[0],self.orientation_quaternion[1],self.orientation_quaternion[2],self.orientation_quaternion[3])) + print('Orientation [Euler Angles]: {} {} {}'.format(self.orientation_euler_angles[0],self.orientation_euler_angles[1],self.orientation_euler_angles[2])) + print('Angular Velocity: {} {} {}'.format(self.angular_velocity[0],self.angular_velocity[1],self.angular_velocity[2])) + print('Linear Acceleration: {} {} {}'.format(self.linear_acceleration[0],self.linear_acceleration[1],self.linear_acceleration[2])) + print('LIDAR Pointcloud: \n{}'.format(self.lidar_pointcloud)) + cv2.imshow(self.id + ' Front Camera Preview', cv2.resize(self.front_camera_image, (640, 360))) + cv2.imshow(self.id + ' Rear Camera Preview', cv2.resize(self.rear_camera_image, (640, 360))) + cv2.waitKey(1) + + # Generate Hunter SE control commands + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------') + print('Transmit Data to Hunter SE: ' + self.id) + print('-------------------------------\n') + # Monitor Hunter SE control commands + if self.cosim_mode == 0: + cosim_mode_str = 'False' + else: + cosim_mode_str = 'True' + print('Co-Simulation Mode: {}'.format(cosim_mode_str)) + print('Position Command: X: {} Y: {} Z: {}'.format(self.posX_command, self.posY_command, self.posZ_command)) + print('Rotation Command: X: {} Y: {} Z: {} Z: {}'.format(self.rotX_command, self.rotY_command, self.rotZ_command, self.rotW_command)) + print('Throttle Command: {}'.format(self.throttle_command)) + print('Steering Command: {}'.format(self.steering_command)) + return {str(self.id) + ' CoSim': str(self.cosim_mode), + str(self.id) + ' PosX': str(self.posX_command), str(self.id) + ' PosY': str(self.posY_command), str(self.id) + ' PosZ': str(self.posZ_command), + str(self.id) + ' RotX': str(self.rotX_command), str(self.id) + ' RotY': str(self.rotY_command), str(self.id) + ' RotZ': str(self.rotZ_command), str(self.id) + ' RotW': str(self.rotW_command), + str(self.id) + ' Throttle': str(self.throttle_command), str(self.id) + ' Steering': str(self.steering_command)} + +################################################################################ + +# OpenCAV class +class OpenCAV: + def __init__(self): + # OpenCAV data + self.id = None + self.collision_count = None + self.throttle = None + self.steering = None + self.brake = None + self.handbrake = None + self.encoder_ticks = None + self.encoder_angles = None + self.position = None + self.orientation_quaternion = None + self.orientation_euler_angles = None + self.angular_velocity = None + self.linear_acceleration = None + # self.lidar_pointcloud = None + self.front_camera_image = None + self.rear_camera_image = None + # OpenCAV commands + self.cosim_mode = None + self.posX_command = None + self.posY_command = None + self.posZ_command = None + self.rotX_command = None + self.rotY_command = None + self.rotZ_command = None + self.rotW_command = None + self.throttle_command = None + self.steering_command = None + self.brake_command = None + self.handbrake_command = None + self.headlights_command = None + self.indicators_command = None + + # Parse OpenCAV sensor data + def parse_data(self, data, verbose=False): + # Collision count + self.collision_count = int(data[self.id + " Collisions"]) + # Actuator feedbacks + self.throttle = float(data[self.id + " Throttle"]) + self.steering = float(data[self.id + " Steering"]) + self.brake = float(data[self.id + " Brake"]) + self.handbrake = float(data[self.id + " Handbrake"]) + # Wheel encoders + self.encoder_ticks = np.fromstring(data[self.id + " Encoder Ticks"], dtype=int, sep=' ') + self.encoder_angles = np.fromstring(data[self.id + " Encoder Angles"], dtype=float, sep=' ') + # IPS + self.position = np.fromstring(data[self.id + " Position"], dtype=float, sep=' ') + # IMU + self.orientation_quaternion = np.fromstring(data[self.id + " Orientation Quaternion"], dtype=float, sep=' ') + self.orientation_euler_angles = np.fromstring(data[self.id + " Orientation Euler Angles"], dtype=float, sep=' ') + self.angular_velocity = np.fromstring(data[self.id + " Angular Velocity"], dtype=float, sep=' ') + self.linear_acceleration = np.fromstring(data[self.id + " Linear Acceleration"], dtype=float, sep=' ') + # LIDAR + # self.lidar_pointcloud = np.frombuffer(base64.b64decode(data[self.id + " LIDAR Pointcloud"]), dtype=np.uint8) + # Cameras + self.left_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Left Camera Image"])))), cv2.COLOR_RGB2BGR) + self.right_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Right Camera Image"])))), cv2.COLOR_RGB2BGR) + if verbose: + print('\n--------------------------------') + print('Receive Data from OpenCAV: ' + self.id) + print('--------------------------------\n') + # Monitor OpenCAV data + print('Collisions: {}'.format(self.collision_count)) + print('Throttle: {}'.format(self.throttle)) + print('Steering: {}'.format(self.steering)) + print('Brake: {}'.format(self.brake)) + print('Handbrake: {}'.format(self.handbrake)) + print('Encoder Ticks: {} {}'.format(self.encoder_ticks[0],self.encoder_ticks[1])) + print('Encoder Angles: {} {}'.format(self.encoder_angles[0],self.encoder_angles[1])) + print('Position: {} {} {}'.format(self.position[0],self.position[1],self.position[2])) + print('Orientation [Quaternion]: {} {} {} {}'.format(self.orientation_quaternion[0],self.orientation_quaternion[1],self.orientation_quaternion[2],self.orientation_quaternion[3])) + print('Orientation [Euler Angles]: {} {} {}'.format(self.orientation_euler_angles[0],self.orientation_euler_angles[1],self.orientation_euler_angles[2])) + print('Angular Velocity: {} {} {}'.format(self.angular_velocity[0],self.angular_velocity[1],self.angular_velocity[2])) + print('Linear Acceleration: {} {} {}'.format(self.linear_acceleration[0],self.linear_acceleration[1],self.linear_acceleration[2])) + # print('LIDAR Pointcloud: \n{}'.format(self.lidar_pointcloud)) + cv2.imshow(self.id + ' Left Camera Preview', cv2.resize(self.left_camera_image, (640, 360))) + cv2.imshow(self.id + ' Right Camera Preview', cv2.resize(self.right_camera_image, (640, 360))) + cv2.waitKey(1) + + # Generate OpenCAV control commands + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------') + print('Transmit Data to OpenCAV: ' + self.id) + print('-------------------------------\n') + # Monitor OpenCAV control commands + if self.cosim_mode == 0: + cosim_mode_str = 'False' + else: + cosim_mode_str = 'True' + print('Co-Simulation Mode: {}'.format(cosim_mode_str)) + print('Position Command: X: {} Y: {} Z: {}'.format(self.posX_command, self.posY_command, self.posZ_command)) + print('Rotation Command: X: {} Y: {} Z: {} Z: {}'.format(self.rotX_command, self.rotY_command, self.rotZ_command, self.rotW_command)) + print('Throttle Command: {}'.format(self.throttle_command)) + print('Steering Command: {}'.format(self.steering_command)) + print('Brake Command: {}'.format(self.brake_command)) + print('Handbrake Command: {}'.format(self.handbrake_command)) + if self.headlights_command == 0: + headlights_cmd_str = 'Disabled' + elif self.headlights_command == 1: + headlights_cmd_str = 'Low Beam' + elif self.headlights_command == 2: + headlights_cmd_str = 'High Beam' + elif self.headlights_command == 3: + headlights_cmd_str = 'Parking Lights' + elif self.headlights_command == 4: + headlights_cmd_str = 'Fog Lights' + elif self.headlights_command == 5: + headlights_cmd_str = 'Low Beam + Parking Lights' + elif self.headlights_command == 6: + headlights_cmd_str = 'Low Beam + Fog Lights' + elif self.headlights_command == 7: + headlights_cmd_str = 'High Beam + Parking Lights' + elif self.headlights_command == 8: + headlights_cmd_str = 'High Beam + Fog Lights' + elif self.headlights_command == 9: + headlights_cmd_str = 'Parking Lights + Fog Lights' + elif self.headlights_command == 10: + headlights_cmd_str = 'Low Beam + Parking Lights + Fog Lights' + elif self.headlights_command == 11: + headlights_cmd_str = 'High Beam + Parking Lights + Fog Lights' + else: + headlights_cmd_str = 'Invalid' + print('Headlights Command: {}'.format(headlights_cmd_str)) + if self.indicators_command == 0: + indicators_cmd_str = 'Disabled' + elif self.indicators_command == 1: + indicators_cmd_str = 'Left Turn Indicator' + elif self.indicators_command == 2: + indicators_cmd_str = 'Right Turn Indicator' + elif self.indicators_command == 3: + indicators_cmd_str = 'Hazard Indicator' + else: + indicators_cmd_str = 'Invalid' + print('Indicators Command: {}'.format(indicators_cmd_str)) + return {str(self.id) + ' CoSim': str(self.cosim_mode), + str(self.id) + ' PosX': str(self.posX_command), str(self.id) + ' PosY': str(self.posY_command), str(self.id) + ' PosZ': str(self.posZ_command), + str(self.id) + ' RotX': str(self.rotX_command), str(self.id) + ' RotY': str(self.rotY_command), str(self.id) + ' RotZ': str(self.rotZ_command), str(self.id) + ' RotW': str(self.rotW_command), + str(self.id) + ' Throttle': str(self.throttle_command), str(self.id) + ' Steering': str(self.steering_command), str(self.id) + ' Brake': str(self.brake_command), str(self.id) + ' Handbrake': str(self.handbrake_command), + str(self.id) + ' Headlights': str(self.headlights_command), str(self.id) + ' Indicators': str(self.indicators_command)} + +################################################################################ + +# RZR class +class RZR: + def __init__(self): + # RZR data + self.id = None + self.collision_count = None + self.throttle = None + self.steering = None + self.brake = None + self.handbrake = None + self.encoder_ticks = None + self.encoder_angles = None + self.position = None + self.orientation_quaternion = None + self.orientation_euler_angles = None + self.angular_velocity = None + self.linear_acceleration = None + # self.lidar_pointcloud = None + self.front_camera_image = None + self.rear_camera_image = None + # RZR commands + self.cosim_mode = None + self.posX_command = None + self.posY_command = None + self.posZ_command = None + self.rotX_command = None + self.rotY_command = None + self.rotZ_command = None + self.rotW_command = None + self.throttle_command = None + self.steering_command = None + self.brake_command = None + self.handbrake_command = None + self.headlights_command = None + + # Parse RZR sensor data + def parse_data(self, data, verbose=False): + # Collision count + self.collision_count = int(data[self.id + " Collisions"]) + # Actuator feedbacks + self.throttle = float(data[self.id + " Throttle"]) + self.steering = float(data[self.id + " Steering"]) + self.brake = float(data[self.id + " Brake"]) + self.handbrake = float(data[self.id + " Handbrake"]) + # Wheel encoders + self.encoder_ticks = np.fromstring(data[self.id + " Encoder Ticks"], dtype=int, sep=' ') + self.encoder_angles = np.fromstring(data[self.id + " Encoder Angles"], dtype=float, sep=' ') + # IPS + self.position = np.fromstring(data[self.id + " Position"], dtype=float, sep=' ') + # IMU + self.orientation_quaternion = np.fromstring(data[self.id + " Orientation Quaternion"], dtype=float, sep=' ') + self.orientation_euler_angles = np.fromstring(data[self.id + " Orientation Euler Angles"], dtype=float, sep=' ') + self.angular_velocity = np.fromstring(data[self.id + " Angular Velocity"], dtype=float, sep=' ') + self.linear_acceleration = np.fromstring(data[self.id + " Linear Acceleration"], dtype=float, sep=' ') + # LIDAR + # self.lidar_pointcloud = np.frombuffer(base64.b64decode(data[self.id + " LIDAR Pointcloud"]), dtype=np.uint8) + # Cameras + self.left_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Left Camera Image"])))), cv2.COLOR_RGB2BGR) + self.right_camera_image = cv2.cvtColor(np.asarray(Image.open(BytesIO(base64.b64decode(data[self.id + " Right Camera Image"])))), cv2.COLOR_RGB2BGR) + if verbose: + print('\n--------------------------------') + print('Receive Data from RZR: ' + self.id) + print('--------------------------------\n') + # Monitor RZR data + print('Collisions: {}'.format(self.collision_count)) + print('Throttle: {}'.format(self.throttle)) + print('Steering: {}'.format(self.steering)) + print('Brake: {}'.format(self.brake)) + print('Handbrake: {}'.format(self.handbrake)) + print('Encoder Ticks: {} {}'.format(self.encoder_ticks[0],self.encoder_ticks[1])) + print('Encoder Angles: {} {}'.format(self.encoder_angles[0],self.encoder_angles[1])) + print('Position: {} {} {}'.format(self.position[0],self.position[1],self.position[2])) + print('Orientation [Quaternion]: {} {} {} {}'.format(self.orientation_quaternion[0],self.orientation_quaternion[1],self.orientation_quaternion[2],self.orientation_quaternion[3])) + print('Orientation [Euler Angles]: {} {} {}'.format(self.orientation_euler_angles[0],self.orientation_euler_angles[1],self.orientation_euler_angles[2])) + print('Angular Velocity: {} {} {}'.format(self.angular_velocity[0],self.angular_velocity[1],self.angular_velocity[2])) + print('Linear Acceleration: {} {} {}'.format(self.linear_acceleration[0],self.linear_acceleration[1],self.linear_acceleration[2])) + # print('LIDAR Pointcloud: \n{}'.format(self.lidar_pointcloud)) + cv2.imshow(self.id + ' Left Camera Preview', cv2.resize(self.left_camera_image, (640, 360))) + cv2.imshow(self.id + ' Right Camera Preview', cv2.resize(self.right_camera_image, (640, 360))) + cv2.waitKey(1) + + # Generate RZR control commands + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------') + print('Transmit Data to RZR: ' + self.id) + print('-------------------------------\n') + # Monitor RZR control commands + if self.cosim_mode == 0: + cosim_mode_str = 'False' + else: + cosim_mode_str = 'True' + print('Co-Simulation Mode: {}'.format(cosim_mode_str)) + print('Position Command: X: {} Y: {} Z: {}'.format(self.posX_command, self.posY_command, self.posZ_command)) + print('Rotation Command: X: {} Y: {} Z: {} Z: {}'.format(self.rotX_command, self.rotY_command, self.rotZ_command, self.rotW_command)) + print('Throttle Command: {}'.format(self.throttle_command)) + print('Steering Command: {}'.format(self.steering_command)) + print('Brake Command: {}'.format(self.brake_command)) + print('Handbrake Command: {}'.format(self.handbrake_command)) + if self.headlights_command == 0: + headlights_cmd_str = 'Disabled' + elif self.headlights_command == 1: + headlights_cmd_str = 'Headlights' + elif self.headlights_command == 2: + headlights_cmd_str = 'Signature Lights' + elif self.headlights_command == 3: + headlights_cmd_str = 'Headlights + Signature Lights' + else: + headlights_cmd_str = 'Invalid' + print('Headlights Command: {}'.format(headlights_cmd_str)) + return {str(self.id) + ' CoSim': str(self.cosim_mode), + str(self.id) + ' PosX': str(self.posX_command), str(self.id) + ' PosY': str(self.posY_command), str(self.id) + ' PosZ': str(self.posZ_command), + str(self.id) + ' RotX': str(self.rotX_command), str(self.id) + ' RotY': str(self.rotY_command), str(self.id) + ' RotZ': str(self.rotZ_command), str(self.id) + ' RotW': str(self.rotW_command), + str(self.id) + ' Throttle': str(self.throttle_command), str(self.id) + ' Steering': str(self.steering_command), str(self.id) + ' Brake': str(self.brake_command), str(self.id) + ' Handbrake': str(self.handbrake_command), + str(self.id) + ' Headlights': str(self.headlights_command)} + +################################################################################ + +# Traffic light class +class TrafficLight: + def __init__(self): + # Traffic light data + self.id = None + self.state = None + # Traffic light command + self.command = None + + # Parse traffic light data + def parse_data(self, data, verbose=False): + # Traffic light state + self.state = int(data[self.id + " State"]) + if verbose: + print('\n--------------------------------------') + print('Receive Data from Traffic Light: ' + self.id) + print('--------------------------------------\n') + # Monitor traffic light data + if self.state == 0: + state_str = 'Disabled' + elif self.state == 1: + state_str = 'Red' + elif self.state == 2: + state_str = 'Yellow' + elif self.state == 3: + state_str = 'Green' + else: + state_str = 'Invalid' + print('Traffic Light State: {}'.format(state_str)) + + # Generate traffic light control commands + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------------') + print('Transmit Data to Traffic Light: ' + self.id) + print('-------------------------------------\n') + # Monitor traffic light control commands + if self.command == 0: + command_str = 'Disabled' + elif self.command == 1: + command_str = 'Red' + elif self.command == 2: + command_str = 'Yellow' + elif self.command == 3: + command_str = 'Green' + else: + command_str = 'Invalid' + print('Traffic Light Command: {}'.format(command_str)) + return {str(self.id) + ' State': str(self.command)} + +################################################################################ + +# Environment class +class Environment: + def __init__(self): + # Environmental conditions + self.auto_time = None + self.time_scale = None + self.time_of_day = None + self.weather_id = None + self.cloud_intensity = None + self.fog_intensity = None + self.rain_intensity = None + self.snow_intensity = None + + # Set environmental conditions + def generate_commands(self, verbose=False): + if verbose: + print('\n-------------------------------------') + print('Set Environmental Conditions:') + print('-------------------------------------\n') + # Monitor environmental conditions + hours = int(self.time_of_day // 60) + minutes = int(self.time_of_day % 60) + seconds = int((self.time_of_day % 1) * 60) + print('Time: {:02d}:{:02d}:{:02d}'.format(hours, minutes, seconds)) + if self.weather_id == 0: + weather_str = 'Custom | Clouds: {}%\tFog: {}%\tRain: {}%\tSnow: {}%'.format(np.round(self.cloud_intensity*100,2), + np.round(self.fog_intensity*100,2), + np.round(self.rain_intensity*100,2), + np.round(self.snow_intensity*1002)) + elif self.weather_id == 1: + weather_str = 'Sunny' + elif self.weather_id == 2: + weather_str = 'Cloudy' + elif self.weather_id == 3: + weather_str = 'Light Fog' + elif self.weather_id == 4: + weather_str = 'Heavy Fog' + elif self.weather_id == 5: + weather_str = 'Light Rain' + elif self.weather_id == 6: + weather_str = 'Heavy Rain' + elif self.weather_id == 7: + weather_str = 'Light Snow' + elif self.weather_id == 8: + weather_str = 'Heavy Snow' + else: + weather_str = 'Invalid' + print('Weather: {}'.format(weather_str)) + return {'Auto Time': str(self.auto_time), 'Time Scale': str(self.time_scale), 'Time': str(self.time_of_day), 'Weather': str(self.weather_id), + 'Clouds': str(self.cloud_intensity), 'Fog': str(self.fog_intensity), 'Rain': str(self.rain_intensity), 'Snow': str(self.snow_intensity)} \ No newline at end of file diff --git a/rzr_aeb.py b/rzr_aeb.py new file mode 100644 index 0000000..f402a7c --- /dev/null +++ b/rzr_aeb.py @@ -0,0 +1,170 @@ +#!/usr/bin/env python + +# Import libraries +import socketio +import eventlet +from flask import Flask +import numpy as np +import cv2 + +import autodrive + +################################################################################ + +# Load YOLO Model +net = cv2.dnn.readNet("yolov3.weights", "yolov3.cfg") + +# Load Classes +with open("coco.names", 'r') as f: + classes = [line.strip() for line in f.readlines()] + +# Configuration +layer_name = net.getLayerNames() +output_layer = [layer_name[i - 1] for i in net.getUnconnectedOutLayers()] +colors = np.random.uniform(0, 255, size=(len(classes), 3)) + +# Initialize environment +environment = autodrive.Environment() + +# Initialize vehicle(s) +rzr_1 = autodrive.RZR() +rzr_1.id = 'V1' + +# Initialize the server +sio = socketio.Server() + +# Flask (web) app +app = Flask(__name__) # '__main__' + +# Registering "connect" event handler for the server +@sio.on('connect') +def connect(sid, environ): + print('Connected!') + +# Registering "Bridge" event handler for the server +@sio.on('Bridge') +def bridge(sid, data): + if data: + + ######################################################################## + # PERCEPTION + ######################################################################## + + # Vehicle data + rzr_1.parse_data(data, verbose=False) + + # Load image + img = cv2.resize(rzr_1.right_camera_image, (640, 360)) + contrast = 1.0 # Contrast control (1.0-3.0) + brightness = 0 # Brightness control (0-100) + img = cv2.convertScaleAbs(img, alpha=contrast, beta=brightness) # Adjusting + height, width, channel = img.shape # Resizing + + # Detect Objects + blob = cv2.dnn.blobFromImage(img, 0.00392, (416, 416), (0, 0, 0), True, crop=False) + net.setInput(blob) + outs = net.forward(output_layer) + + # Display object detection information + label = None + confidence = None + size = None + class_ids = [] + confidences = [] + boxes = [] + for out in outs: + for detection in out: + scores = detection[5:] + class_id = np.argmax(scores) + confidence = scores[class_id] + if confidence > 0.5: + center_x = int(detection[0] * width) + center_y = int(detection[1] * height) + w = int(detection[2] * width) + h = int(detection[3] * height) + x = int(center_x - w/2) + y = int(center_y - h/2) + boxes.append([x, y, w, h]) + confidences.append(float(confidence)) + class_ids.append(class_id) + indices = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.4) + font = cv2.FONT_HERSHEY_PLAIN + for i in range(len(boxes)): + if i in indices: + x, y, w, h = boxes[i] + label = str(classes[class_ids[i]]) + confidence = np.round(confidences[i]*100, 2) + size = w*h + print('Class: {} \t Confidence: {} % \t Size: {} px²'.format(label, confidence, size)) + color = colors[i] + cv2.rectangle(img, (x, y), (x + w, y + h), color, 2) + cv2.putText(img, label, (x, y + 30), font, 3, color, 3) + cv2.imshow("Object Detection", img) + cv2.waitKey(1) + + ######################################################################## + # PLANNING + ######################################################################## + + # Compute distance to collision and AEB trigger + DTC = np.linalg.norm(rzr_1.position - np.array([483.45, 44.81, 351.41])) + AEB = 1 if (label=="animal" and confidence>=50 and size>=4000) else 0 + + ######################################################################## + # CONTROL + ######################################################################## + + # Environmental conditions + environment.auto_time = "False" # ["False", "True"] + environment.time_scale = 60 # [0, inf) (only used if auto_time==True) + environment.time_of_day = 560 # [minutes in 24 hour format] (only used if auto_time==False) + environment.weather_id = 3 # [0=Custom, 1=Sunny, 2=Cloudy, 3=LightFog, 4=HeavyFog, 5=LightRain, 6=HeavyRain, 7=LightSnow, 8=HeavySnow] + environment.cloud_intensity = 0.0 # [0, 1] (only used if weather_id==0) + environment.fog_intensity = 0.0 # [0, 1] (only used if weather_id==0) + environment.rain_intensity = 0.0 # [0, 1] (only used if weather_id==0) + environment.snow_intensity = 0.0 # [0, 1] (only used if weather_id==0) + + # Vehicle co-simulation mode + rzr_1.cosim_mode = 0 + + # Vehicle actuator commands (only if cosim_mode==0) + if AEB == 1: + rzr_1.throttle_command = 0 # [-1, 1] + rzr_1.steering_command = 0 # [-1, 1] + rzr_1.brake_command = 1 # [0, 1] + rzr_1.handbrake_command = 0 # [0, 1] + else: + rzr_1.throttle_command = 0.25 # [-1, 1] + rzr_1.steering_command = 0 # [-1, 1] + rzr_1.brake_command = 0 # [0, 1] + rzr_1.handbrake_command = 0 # [0, 1] + + # Vehicle light commands + if (0 <= environment.time_of_day <= 420 or 1080 <= environment.time_of_day <= 1440): # Night + if (environment.weather_id!=1 or environment.weather_id!=2 or (environment.weather_id==0 and environment.fog_intensity != 0)): # Foggy night + rzr_1.headlights_command = 3 # Vehicle headlights command [0 = Disabled, 1 = Enabled, 2 = Signature Lights, 3 = 1+2] + else: # Clear night + rzr_1.headlights_command = 3 # Vehicle headlights command [0 = Disabled, 1 = Enabled, 2 = Signature Lights, 3 = 1+2] + elif (environment.weather_id!=1 or environment.weather_id!=2 or (environment.weather_id==0 and environment.fog_intensity != 0)): # Foggy day + rzr_1.headlights_command = 2 # Vehicle headlights command [0 = Disabled, 1 = Enabled, 2 = Signature Lights, 3 = 1+2] + else: # Clear day + rzr_1.headlights_command = 2 # Vehicle headlights command [0 = Disabled, 1 = Enabled, 2 = Signature Lights, 3 = 1+2] + + # Verbose + print("DTC: {} m\tAEB: {}".format(np.round(DTC, 2), AEB==1)) + + ######################################################################## + + json_msg = environment.generate_commands(verbose=False) # Generate environment message + json_msg.update(rzr_1.generate_commands(verbose=False)) # Append vehicle 1 message + + try: + sio.emit('Bridge', data=json_msg) + except Exception as exception_instance: + print(exception_instance) + +################################################################################ + +if __name__ == '__main__': + app = socketio.Middleware(sio, app) # Wrap flask application with socketio's middleware + eventlet.wsgi.server(eventlet.listen(('', 4567)), app) # Deploy as an eventlet WSGI server \ No newline at end of file diff --git a/simulation_stream_recorder.sh b/simulation_stream_recorder.sh deleted file mode 100644 index 68ef574..0000000 --- a/simulation_stream_recorder.sh +++ /dev/null @@ -1,56 +0,0 @@ -#!/bin/bash - -# Usage check: at least one argument (mode) and optionally handles more arguments based on the mode -if [ "$#" -lt 1 ]; then - echo "Usage: $0 [output_filename] [simulation_duration_seconds]" - echo "Modes:" - echo " record - Records to MP4 file. Requires output_filename and simulation_duration_seconds." - echo " stream - Streams to HLS .m3u8 file. Requires output_filename." - exit 1 -fi - -mode=$1 -output_filename=$2 -simulation_duration=$3 -script_dir="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" -output_directory="$script_dir/output" - -# Create virtual display (:97) -Xvfb :97 -screen 0 640x360x24 & - -# Export the display environment variable to point to the virtual display -export DISPLAY=:97 - -# Launch the AutoDRIVE Simulator application with output directed to the virtual display -$script_dir/AutoDRIVE\ Simulator.x86_64 >/dev/null 2>&1 & - -# Wait for a moment to ensure the simulator is properly launched -sleep 5 - -case $mode in - record) - if [ "$#" -ne 3 ]; then - echo "Recording mode requires 3 arguments: mode, output_filename, simulation_duration_seconds" - exit 1 - fi - # Record video from the virtual display using ffmpeg and save it to a video file - ffmpeg -f x11grab -video_size 640x360 -i :97 -t $simulation_duration -c:v libx265 -crf 28 -preset medium "$output_directory/$output_filename" - echo "Video recording completed and saved to $output_directory/$output_filename" - ;; - stream) - if [ "$#" -ne 2 ]; then - echo "Streaming mode requires 2 arguments: mode, output_filename" - exit 1 - fi - # Stream to HLS using ffmpeg (assuming output_filename is the base name for the HLS playlist) - ffmpeg -f x11grab -s 640x360 -i :97 -c:v libx264 -preset ultrafast -f hls -hls_time 10 -hls_list_size 0 -hls_segment_filename "$output_directory/$output_filename.%03d.ts" "$output_directory/$output_filename.m3u8" & - - - echo "Streaming started and available at $output_directory/$output_filename.m3u8" - ;; - *) - echo "Invalid mode: $mode" - echo "Valid modes are: record, stream" - exit 1 - ;; -esac diff --git a/yolov2-tiny.cfg b/yolov2-tiny.cfg new file mode 100644 index 0000000..81d0ac4 --- /dev/null +++ b/yolov2-tiny.cfg @@ -0,0 +1,139 @@ +[net] +# Testing +batch=1 +subdivisions=1 +# Training +# batch=64 +# subdivisions=2 +width=416 +height=416 +channels=3 +momentum=0.9 +decay=0.0005 +angle=0 +saturation = 1.5 +exposure = 1.5 +hue=.1 + +learning_rate=0.001 +burn_in=1000 +max_batches = 500200 +policy=steps +steps=400000,450000 +scales=.1,.1 + +[convolutional] +batch_normalize=1 +filters=16 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=1 + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +########### + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=425 +activation=linear + +[region] +anchors = 0.57273, 0.677385, 1.87446, 2.06253, 3.33843, 5.47434, 7.88282, 3.52778, 9.77052, 9.16828 +bias_match=1 +classes=80 +coords=4 +num=5 +softmax=1 +jitter=.2 +rescore=0 + +object_scale=5 +noobject_scale=1 +class_scale=1 +coord_scale=1 + +absolute=1 +thresh = .6 +random=1 diff --git a/yolov2.cfg b/yolov2.cfg new file mode 100644 index 0000000..088edf8 --- /dev/null +++ b/yolov2.cfg @@ -0,0 +1,258 @@ +[net] +# Testing +batch=1 +subdivisions=1 +# Training +# batch=64 +# subdivisions=8 +width=608 +height=608 +channels=3 +momentum=0.9 +decay=0.0005 +angle=0 +saturation = 1.5 +exposure = 1.5 +hue=.1 + +learning_rate=0.001 +burn_in=1000 +max_batches = 500200 +policy=steps +steps=400000,450000 +scales=.1,.1 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + + +####### + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[route] +layers=-9 + +[convolutional] +batch_normalize=1 +size=1 +stride=1 +pad=1 +filters=64 +activation=leaky + +[reorg] +stride=2 + +[route] +layers=-1,-4 + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=425 +activation=linear + + +[region] +anchors = 0.57273, 0.677385, 1.87446, 2.06253, 3.33843, 5.47434, 7.88282, 3.52778, 9.77052, 9.16828 +bias_match=1 +classes=80 +coords=4 +num=5 +softmax=1 +jitter=.3 +rescore=1 + +object_scale=5 +noobject_scale=1 +class_scale=1 +coord_scale=1 + +absolute=1 +thresh = .6 +random=1 diff --git a/yolov3-tiny.cfg b/yolov3-tiny.cfg new file mode 100644 index 0000000..cfca3cf --- /dev/null +++ b/yolov3-tiny.cfg @@ -0,0 +1,182 @@ +[net] +# Testing +batch=1 +subdivisions=1 +# Training +# batch=64 +# subdivisions=2 +width=416 +height=416 +channels=3 +momentum=0.9 +decay=0.0005 +angle=0 +saturation = 1.5 +exposure = 1.5 +hue=.1 + +learning_rate=0.001 +burn_in=1000 +max_batches = 500200 +policy=steps +steps=400000,450000 +scales=.1,.1 + +[convolutional] +batch_normalize=1 +filters=16 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=2 + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[maxpool] +size=2 +stride=1 + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +########### + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + + +[yolo] +mask = 3,4,5 +anchors = 10,14, 23,27, 37,58, 81,82, 135,169, 344,319 +classes=80 +num=6 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +random=1 + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = -1, 8 + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + +[yolo] +mask = 0,1,2 +anchors = 10,14, 23,27, 37,58, 81,82, 135,169, 344,319 +classes=80 +num=6 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +random=1 diff --git a/yolov3.cfg b/yolov3.cfg new file mode 100644 index 0000000..938ffff --- /dev/null +++ b/yolov3.cfg @@ -0,0 +1,789 @@ +[net] +# Testing +# batch=1 +# subdivisions=1 +# Training +batch=64 +subdivisions=16 +width=608 +height=608 +channels=3 +momentum=0.9 +decay=0.0005 +angle=0 +saturation = 1.5 +exposure = 1.5 +hue=.1 + +learning_rate=0.001 +burn_in=1000 +max_batches = 500200 +policy=steps +steps=400000,450000 +scales=.1,.1 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=leaky + +# Downsample + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=2 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=32 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +# Downsample + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=2 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +# Downsample + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=2 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +# Downsample + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=2 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +# Downsample + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=2 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=1 +pad=1 +activation=leaky + +[shortcut] +from=-3 +activation=linear + +###################### + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + +[yolo] +mask = 6,7,8 +anchors = 10,13, 16,30, 33,23, 30,61, 62,45, 59,119, 116,90, 156,198, 373,326 +classes=80 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +random=1 + + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = -1, 61 + + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + +[yolo] +mask = 3,4,5 +anchors = 10,13, 16,30, 33,23, 30,61, 62,45, 59,119, 116,90, 156,198, 373,326 +classes=80 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +random=1 + + + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = -1, 36 + + + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=255 +activation=linear + + +[yolo] +mask = 0,1,2 +anchors = 10,13, 16,30, 33,23, 30,61, 62,45, 59,119, 116,90, 156,198, 373,326 +classes=80 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +random=1 +