diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 34546074..86d9c5f4 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -9,7 +9,8 @@ about: Create a report to help us improve -## Bug report +## Bug report + - AutonomySim Version/#commit: - Unreal Engine version: @@ -17,14 +18,17 @@ about: Create a report to help us improve - Operating system version: ### What's the issue you encountered? + ### Settings + ### How can the issue be reproduced? + diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md index 32de27c3..04be1bc5 100644 --- a/.github/ISSUE_TEMPLATE/feature_request.md +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -8,13 +8,17 @@ about: Suggest an idea for this project ## What feature are you suggesting? + ### Overview: + -### Smaller Details: +### Smaller Details: + -### Nature of Request: +### Nature of Request: + - Addition - Change diff --git a/.github/ISSUE_TEMPLATE/question.md b/.github/ISSUE_TEMPLATE/question.md index 1af3e6ac..f46d0344 100644 --- a/.github/ISSUE_TEMPLATE/question.md +++ b/.github/ISSUE_TEMPLATE/question.md @@ -9,13 +9,17 @@ about: Your question may be moved to https://github.com/nervosys/AutonomySim/dis ## Question + ### What's your question? + ### Include context on what you are trying to achieve + #### Context details + diff --git a/.github/workflows/test_docs.yml b/.github/workflows/test_docs.yml index 720ff219..82bef6fa 100644 --- a/.github/workflows/test_docs.yml +++ b/.github/workflows/test_docs.yml @@ -1,6 +1,14 @@ name: "Build and Deploy: Documentation (Doxygen, Sphinx, MkDocs)" -on: [push, pull_request, workflow_dispatch] +on: + push: + branches: + - master + pull_request: + branches: + - master + types: [opened, synchronize, reopened, closed] + # workflow_dispatch: jobs: build-deploy-docs: diff --git a/.github/workflows/test_formatting.yml b/.github/workflows/test_formatting.yml index b270d34f..5ab0bb27 100644 --- a/.github/workflows/test_formatting.yml +++ b/.github/workflows/test_formatting.yml @@ -1,10 +1,33 @@ -name: "Test: C/C++ Source Formatting (clang-format)" +--- +#---------------------------------------------------------------------------------------- +# FILENAME: +# test_formatting.yml +# DESCRIPTION: +# GitHub Actions configuration for clang-format. +# AUTHOR: +# Adam Erickson (Nervosys) +# DATE: +# 2024-02-20 +# NOTES: +# +# Copyright © 2024 Nervosys, LLC +#---------------------------------------------------------------------------------------- -on: [push, pull_request] +name: "Test: Source Code Formatting" + +on: + push: + branches: + - master + pull_request: + branches: + - master + types: [opened, synchronize, reopened, closed] + # workflow_dispatch: jobs: - test-formatting: - name: Formatting Check + test-formatting-cpp: + name: "Test C/C++ Formatting" runs-on: ubuntu-latest strategy: matrix: @@ -44,3 +67,12 @@ jobs: check-path: ${{ matrix.path['check'] }} exclude-regex: ${{ matrix.path['exclude'] }} fallback-style: "LLVM" # optional + test-formatting-python: + name: "Test Python Code Formatting" + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - name: "Run autopep8 style check." + uses: peter-evans/autopep8@v2 + with: + args: --recursive --in-place --aggressive --aggressive ./python/ diff --git a/.github/workflows/test_macos.yml b/.github/workflows/test_macos.yml index 6e0be7dc..419b98ce 100644 --- a/.github/workflows/test_macos.yml +++ b/.github/workflows/test_macos.yml @@ -1,6 +1,30 @@ +--- +#---------------------------------------------------------------------------------------- +# FILENAME: +# test_macos.yml +# DESCRIPTION: +# GitHub Actions configuration for macOS. +# AUTHOR: +# Adam Erickson (Nervosys) +# DATE: +# 2024-02-20 +# NOTES: +# - Homebrew is installed by default. +# +# Copyright © 2024 Nervosys, LLC +#---------------------------------------------------------------------------------------- + name: "Build: macOS" -on: [push, pull_request, workflow_dispatch] # triggers +on: + push: + branches: + - master + pull_request: + branches: + - master + types: [opened, synchronize, reopened, closed] + # workflow_dispatch: jobs: build-macos: @@ -8,12 +32,13 @@ jobs: strategy: matrix: os: [macos-11, macos-12, macos-13, macos-14] # versions: [11: Big Sur, 12: Monterey, 13: Ventura, 14: Sonoma] - - # Steps represent a sequence of tasks that will be executed as part of the job + # arch: + # - amd64 + # - arm64 steps: - uses: actions/checkout@v4 - - name: "Install BASH (newer macOS versions use Zsh by default)" + - name: "Install BASH (macOS version > 10.14 uses Zsh by default)" run: brew install bash - name: "Remove Python 3 for Python 3.11 symbolic link" diff --git a/.github/workflows/test_ubuntu.yml b/.github/workflows/test_ubuntu.yml index 38b7d4b6..72a4211b 100644 --- a/.github/workflows/test_ubuntu.yml +++ b/.github/workflows/test_ubuntu.yml @@ -1,6 +1,29 @@ +--- +#---------------------------------------------------------------------------------------- +# FILENAME: +# test_linux.yml +# DESCRIPTION: +# GitHub Actions configuration for Linux. +# AUTHOR: +# Adam Erickson (Nervosys) +# DATE: +# 2024-02-20 +# NOTES: +# +# Copyright © 2024 Nervosys, LLC +#---------------------------------------------------------------------------------------- + name: "Build: Linux" -on: [push, pull_request, workflow_dispatch] +on: + push: + branches: + - master + pull_request: + branches: + - master + types: [opened, synchronize, reopened, closed] + # workflow_dispatch: jobs: build-ubuntu: @@ -11,8 +34,6 @@ jobs: arch: - amd64 # - arm64 - - # sequence of tasks that will be executed as part of the job steps: - uses: actions/checkout@v4 @@ -24,8 +45,8 @@ jobs: shell: bash run: ./scripts/build.sh - - name: "Build ROS2 wrapper" - # if: matrix.os == 'ubuntu-20.04' + - name: "Build ROS2 Wrapper" + # if: ${{ matrix.os == 'ubuntu-20.04' }} shell: bash run: | bash ./scripts/install_ros2_deps.sh diff --git a/.github/workflows/test_windows.yml b/.github/workflows/test_windows.yml index 54f359c9..caa6d108 100644 --- a/.github/workflows/test_windows.yml +++ b/.github/workflows/test_windows.yml @@ -1,3 +1,19 @@ +--- +#---------------------------------------------------------------------------------------- +# FILENAME: +# test_windows.yml +# DESCRIPTION: +# GitHub Actions configuration for Windows. +# AUTHOR: +# Adam Erickson (Nervosys) +# DATE: +# 2024-02-20 +# NOTES: +# - Requires Visual Studio 2022 Build Tools v143 (windows-2022)? +# +# Copyright © 2024 Nervosys, LLC +#---------------------------------------------------------------------------------------- + name: "Build: Windows" on: @@ -8,34 +24,42 @@ on: branches: - master types: [opened, synchronize, reopened, closed] + # workflow_dispatch: jobs: build-windows: runs-on: ${{ matrix.os }} # windows-latest strategy: matrix: - # NOTE: Requires Visual Studio 2022 Build Tools v143 (windows-2022) os: [windows-2022] # versions: [2019: 10, 2022: 11] arch: - amd64 # - arm64 steps: - uses: actions/checkout@v4 - - name: "Add MSBuild to PATH" - uses: microsoft/setup-msbuild@v2 + + # - name: "Add MSBuild to PATH" + # uses: microsoft/setup-msbuild@v2 + # - name: "Enable Developer Command Prompt for Visual Studio" # uses: ilammy/msvc-dev-cmd@v1 - # - name: "Enable Developer Command Prompt for Visual Studio 2019" - # if: ${{ matrix.os == 'windows-2019' }} - # shell: pwsh - # run: 'C:\Program Files (x86)\Microsoft Visual Studio\2019\Community\Common7\Tools\Launch-VsDevShell.ps1' - # - name: "Enable Developer Command Prompt for Visual Studio 2022" - # if: ${{ matrix.os == 'windows-2022' }} - # shell: pwsh - # run: 'C:\Program Files\Microsoft Visual Studio\2022\Community\Common7\Tools\Launch-VsDevShell.ps1' + + - name: "Activate Developer PowerShell for Visual Studio 2019" + if: ${{ matrix.os == 'windows-2019' }} + shell: pwsh + run: | + 'C:\Program Files (x86)\Microsoft Visual Studio\2019\Enterprise\Common7\Tools\Launch-VsDevShell.ps1' + $CMAKE_GENERATOR_PLATFORM = 'Visual Studio 16 2019' + + - name: "Activate Developer PowerShell for Visual Studio 2022" + if: ${{ matrix.os == 'windows-2022' }} + shell: pwsh + run: | + 'C:\Program Files\Microsoft Visual Studio\2022\Enterprise\Common7\Tools\Launch-VsDevShell.ps1' + $CMAKE_GENERATOR_PLATFORM = 'Visual Studio 17 2022' + - name: "Build AutonomySim" shell: pwsh run: | # Set-PSDebug -Trace 1 - # $CMAKE_GENERATOR_PLATFORM = 'Visual Studio 17 2022' .\scripts\build.ps1 -BuildMode 'Release' -IntegrateDeploy diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt index 17e8c19b..c60c05b6 100644 --- a/cmake/CMakeLists.txt +++ b/cmake/CMakeLists.txt @@ -3,7 +3,7 @@ project(AutonomySim) add_subdirectory("AutonomyLib") add_subdirectory("AutonomyLibUnitTests") -add_subdirectory("rpclib_wrapper") +add_subdirectory("RpcLibWrapper") add_subdirectory("MavLinkCom") add_subdirectory("HelloCar") add_subdirectory("HelloDrone") diff --git a/cmake/rpclib_wrapper/CMakeLists.txt b/cmake/RpcLibWrapper/CMakeLists.txt similarity index 95% rename from cmake/rpclib_wrapper/CMakeLists.txt rename to cmake/RpcLibWrapper/CMakeLists.txt index 60a1f415..5b6e52c5 100644 --- a/cmake/rpclib_wrapper/CMakeLists.txt +++ b/cmake/RpcLibWrapper/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5.0) -project(rpclib_wrapper) +project(RpcLibWrapper) list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../cmake-modules") include("${CMAKE_CURRENT_LIST_DIR}/../cmake-modules/CommonSetup.cmake") diff --git a/python/autonomysim/ai/imitation/__init__.py b/python/autonomysim/ai/imitation/__init__.py index a4c755bf..932586df 100644 --- a/python/autonomysim/ai/imitation/__init__.py +++ b/python/autonomysim/ai/imitation/__init__.py @@ -6,54 +6,54 @@ # Imitation Learning -This section is about training a model to steer our Formula car using imitation learning. -The code in this section is based on the [Autonomous Driving Cookbook](https://github.com/nervosys/AutonomousDrivingCookbook/tree/master/AutonomySimE2EDeepLearning) from AutonomySim and it's highly recommended to read the tutorial first. +This section is about training a model to steer our Formula car using imitation learning. +The code in this section is based on the [Autonomous Driving Cookbook](https://github.com/nervosys/AutonomousDrivingCookbook/tree/master/AutonomySimE2EDeepLearning) from AutonomySim and it's highly recommended to read the tutorial first. ## Prerequisites -* Operating system: Windows 10 -* GPU: Nvidia GTX 1080 or higher (recommended) -* Software: Unreal Engine 4.24 and Visual Studio 2019 (see [upgrade instructions](../../docs/unreal_upgrade.md)) -* Development: CUDA 9.0 and python 3.5. -* Python libraries: Keras 2.1.2, TensorFlow 1.6.0. -* Note: Newer versions of keras or tensorflow are recommended but can cause syntax errors. - +* Operating system: Windows 10 +* GPU: Nvidia GTX 1080 or higher (recommended) +* Software: Unreal Engine 4.24 and Visual Studio 2019 (see [upgrade instructions](../../docs/unreal_upgrade.md)) +* Development: CUDA 9.0 and python 3.5. +* Python libraries: Keras 2.1.2, TensorFlow 1.6.0. +* Note: Newer versions of keras or tensorflow are recommended but can cause syntax errors. + ## What's inside - -![imitation learning](https://github.com/nervosys/AutonomySim/wiki/images/technion/imitation_learning_example.gif) -*Driving in simulation using trained imitation learning model, based on recorded data* -Imitation learning includes the usage of labeled data as input to a training algorithm with the purpose of having the algorithm imitate the actions of people who recorded the data. +![imitation learning](https://github.com/nervosys/AutonomySim/wiki/images/technion/imitation_learning_example.gif) +*Driving in simulation using trained imitation learning model, based on recorded data* + +Imitation learning includes the usage of labeled data as input to a training algorithm with the purpose of having the algorithm imitate the actions of people who recorded the data. ![diagram](https://github.com/nervosys/AutonomySim/wiki/images/technion/imitation_diagram.PNG) -This diagram is represented by these files: +This diagram is represented by these files: -**cook_data.py** -This file is responsible for preparing .h5 dataset files for the training procedure. -The code rely on having two adjacent folders: -'raw_data' - contains folders of recorded data by AutonomySim's recording method. -'cooked_data' - empty folder to store the .h5 files. +**cook_data.py** +This file is responsible for preparing .h5 dataset files for the training procedure. +The code rely on having two adjacent folders: +'raw_data' - contains folders of recorded data by AutonomySim's recording method. +'cooked_data' - empty folder to store the .h5 files. -The flag "COOK_ALL_DATA" gives the option to choose all subfolders, or exclude some of them. +The flag "COOK_ALL_DATA" gives the option to choose all subfolders, or exclude some of them. -**train_model.py** -This file is responsible to train a model using the .h5 dataset files. -The code rely on having two adjacent folders: -'cooked_data' - contains the .h5 dataset files. -'models' - empty folder to store the generated models. +**train_model.py** +This file is responsible to train a model using the .h5 dataset files. +The code rely on having two adjacent folders: +'cooked_data' - contains the .h5 dataset files. +'models' - empty folder to store the generated models. -The file will preprocess the data, add augmentations and create a neural network model that predicts the next steering angle. +The file will preprocess the data, add augmentations and create a neural network model that predicts the next steering angle. -**drive_model.py** -This file connects to the simulation in order to upload a trained model and drive using it. +**drive_model.py** +This file connects to the simulation in order to upload a trained model and drive using it. By using the predicted steering value, the code calculates related control parameters and maintain driving with steady velocities. ## Training Tips -We recommend on using augmentation and recording techniques. -We give here an example for two methods: -- [CycleLight](https://github.com/nervosys/AutonomySim/wiki/graphic_features) - Animation of a day light cycle in a changeable, potentially very short period of time. -- Shifted images - Altering the camera’s position to the right or the left of the car, so that it can record images in extreme conditions. To simulate driving back to the center from those extreme situations, post-process the recorded angle of the steering accordingly (manually). +We recommend on using augmentation and recording techniques. +We give here an example for two methods: +- [CycleLight](https://github.com/nervosys/AutonomySim/wiki/graphic_features) - Animation of a day light cycle in a changeable, potentially very short period of time. +- Shifted images - Altering the camera’s position to the right or the left of the car, so that it can record images in extreme conditions. To simulate driving back to the center from those extreme situations, post-process the recorded angle of the steering accordingly (manually). """ diff --git a/python/autonomysim/ai/imitation/agents.py b/python/autonomysim/ai/imitation/agents.py index 61ac91ac..8e415455 100644 --- a/python/autonomysim/ai/imitation/agents.py +++ b/python/autonomysim/ai/imitation/agents.py @@ -1,14 +1,12 @@ +from autonomysim.types import ImageRequest, ImageType +from autonomysim.clients import CarClient, CarControls +from keras.models import load_model +import numpy as np +import time import os os.environ["TF_CPP_MIN_LOG_LEVEL"] = "2" -import time -import numpy as np -from keras.models import load_model - -from autonomysim.clients import CarClient, CarControls -from autonomysim.types import ImageRequest, ImageType - # Trained model path MODEL_PATH = "./models/example_model.h5" @@ -28,8 +26,11 @@ def get_image(self, client): image_response = client.simGetImages( [ImageRequest("0", ImageType.Scene, False, False)] )[0] - image1d = np.fromstring(image_response.image_data_uint8, dtype=np.uint8) - image_rgb = image1d.reshape(image_response.height, image_response.width, 3) + image1d = np.fromstring( + image_response.image_data_uint8, + dtype=np.uint8) + image_rgb = image1d.reshape( + image_response.height, image_response.width, 3) return image_rgb[78:144, 27:227, 0:2].astype(float) def load(self, model_path=MODEL_PATH): @@ -61,7 +62,8 @@ def run(self): while True: # Update throttle value according to steering angle if abs(car_controls.steering) <= 1.0: - car_controls.throttle = 0.8 - (0.4 * abs(car_controls.steering)) + car_controls.throttle = 0.8 - \ + (0.4 * abs(car_controls.steering)) else: car_controls.throttle = 0.4 @@ -76,7 +78,8 @@ def run(self): end_time = time.time() received_output = model_output[0][0] - # Rescale prediction to [-1,1] and factor by 0.82 for drive smoothness + # Rescale prediction to [-1,1] and factor by 0.82 for drive + # smoothness car_controls.steering = round( (0.82 * (float((model_output[0][0] * 2.0) - 1))), 2 ) diff --git a/python/autonomysim/ai/imitation/generators.py b/python/autonomysim/ai/imitation/generators.py index a9a01c4b..0a1d2508 100644 --- a/python/autonomysim/ai/imitation/generators.py +++ b/python/autonomysim/ai/imitation/generators.py @@ -115,18 +115,14 @@ def random_transform_with_states(self, x): theta = 0 if self.height_shift_range: - tx = ( - np.random.uniform(-self.height_shift_range, self.height_shift_range) - * x.shape[img_row_axis] - ) + tx = (np.random.uniform(-self.height_shift_range, + self.height_shift_range) * x.shape[img_row_axis]) else: tx = 0 if self.width_shift_range: - ty = ( - np.random.uniform(-self.width_shift_range, self.width_shift_range) - * x.shape[img_col_axis] - ) + ty = (np.random.uniform(-self.width_shift_range, + self.width_shift_range) * x.shape[img_col_axis]) else: ty = 0 @@ -138,7 +134,8 @@ def random_transform_with_states(self, x): if self.zoom_range[0] == 1 and self.zoom_range[1] == 1: zx, zy = 1, 1 else: - zx, zy = np.random.uniform(self.zoom_range[0], self.zoom_range[1], 2) + zx, zy = np.random.uniform( + self.zoom_range[0], self.zoom_range[1], 2) transform_matrix = None if theta != 0: @@ -278,17 +275,21 @@ def __init__( ) channels_axis = 3 if data_format == "channels_last" else 1 if self.x_images.shape[channels_axis] not in {1, 3, 4}: - raise ValueError( - "NumpyArrayIterator is set to use the " - 'data format convention "' + data_format + '" ' - "(channels on axis " + str(channels_axis) + "), i.e. expected " - "either 1, 3 or 4 channels on axis " + str(channels_axis) + ". " - "However, it was passed an array with shape " - + str(self.x_images.shape) - + " (" - + str(self.x_images.shape[channels_axis]) - + " channels)." - ) + raise ValueError("NumpyArrayIterator is set to use the " + 'data format convention "' + + data_format + + '" ' + "(channels on axis " + + str(channels_axis) + + "), i.e. expected " + "either 1, 3 or 4 channels on axis " + + str(channels_axis) + + ". " + "However, it was passed an array with shape " + + str(self.x_images.shape) + + " (" + + str(self.x_images.shape[channels_axis]) + + " channels).") if x_prev_states is not None: self.x_prev_states = x_prev_states else: @@ -342,7 +343,7 @@ def __get_indexes(self, index_array): if self.roi is not None: batch_x_images = batch_x_images[ - :, self.roi[0] : self.roi[1], self.roi[2] : self.roi[3], : + :, self.roi[0]: self.roi[1], self.roi[2]: self.roi[3], : ] used_indexes = [] @@ -352,12 +353,11 @@ def __get_indexes(self, index_array): if self.roi is not None: x_images = x_images[ - self.roi[0] : self.roi[1], self.roi[2] : self.roi[3], : + self.roi[0]: self.roi[1], self.roi[2]: self.roi[3], : ] transformed = self.image_data_generator.random_transform_with_states( - x_images.astype(K.floatx()) - ) + x_images.astype(K.floatx())) x_images = transformed[0] is_horiz_flipped.append(transformed[1]) x_images = self.image_data_generator.standardize(x_images) @@ -386,8 +386,7 @@ def __get_indexes(self, index_array): batch_x_images[i], self.data_format, scale=True ) fname = "{prefix}_{index}_{hash}.{format}".format( - prefix=self.save_prefix, index=1, hash=hash, format=self.save_format - ) + prefix=self.save_prefix, index=1, hash=hash, format=self.save_format) img.save(os.path.join(self.save_to_dir, fname)) batch_y = self.y[list(sorted(used_indexes))] @@ -401,7 +400,8 @@ def __get_indexes(self, index_array): if np.isclose(batch_y[i], 0.5, rtol=0.005, atol=0.005): num_of_close_samples += 1 - if np.random.uniform(low=0, high=1) > self.zero_drop_percentage: + if np.random.uniform( + low=0, high=1) > self.zero_drop_percentage: idx.append(True) else: idx.append(False) @@ -410,7 +410,8 @@ def __get_indexes(self, index_array): idx.append(True) else: if batch_y[i][int(len(batch_y[i]) / 2)] == 1: - if np.random.uniform(low=0, high=1) > self.zero_drop_percentage: + if np.random.uniform( + low=0, high=1) > self.zero_drop_percentage: idx.append(True) else: idx.append(False) diff --git a/python/autonomysim/ai/imitation/preprocessors.py b/python/autonomysim/ai/imitation/preprocessors.py index e30116ed..90844ca3 100644 --- a/python/autonomysim/ai/imitation/preprocessors.py +++ b/python/autonomysim/ai/imitation/preprocessors.py @@ -64,7 +64,12 @@ def readImagesFromPath(image_names): return returnValue -def splitTrainValidationAndTestData(all_data_mappings, split_ratio=(0.7, 0.2, 0.1)): +def splitTrainValidationAndTestData( + all_data_mappings, + split_ratio=( + 0.7, + 0.2, + 0.1)): """Simple function to create train, validation and test splits on the data. Inputs: @@ -108,7 +113,11 @@ def generateDataMapAutonomySim(folders, velocity_max=MAX_SPEED): all_mappings = {} for folder in folders: print("Reading data from {0}...".format(folder)) - current_df = pd.read_csv(os.path.join(folder, "autonomysim_rec.txt"), sep="\t") + current_df = pd.read_csv( + os.path.join( + folder, + "autonomysim_rec.txt"), + sep="\t") for i in range(1, current_df.shape[0] - 1): if ( @@ -126,12 +135,16 @@ def generateDataMapAutonomySim(folders, velocity_max=MAX_SPEED): previous_state = norm_steering + norm_throttle + norm_speed # Append lists - # compute average steering over 3 consecutive recorded images, this will serve as the label + # compute average steering over 3 consecutive recorded images, this + # will serve as the label - norm_steering0 = (float(current_df.iloc[i][["Steering"]]) + 1) / 2.0 - norm_steering1 = (float(current_df.iloc[i + 1][["Steering"]]) + 1) / 2.0 + norm_steering0 = ( + float(current_df.iloc[i][["Steering"]]) + 1) / 2.0 + norm_steering1 = ( + float(current_df.iloc[i + 1][["Steering"]]) + 1) / 2.0 - temp_sum_steering = norm_steering[0] + norm_steering0 + norm_steering1 + temp_sum_steering = norm_steering[0] + \ + norm_steering0 + norm_steering1 average_steering = temp_sum_steering / 3.0 current_label = [average_steering] @@ -142,8 +155,7 @@ def generateDataMapAutonomySim(folders, velocity_max=MAX_SPEED): if image_filepath in all_mappings: print( - "Error: attempting to add image {0} twice.".format(image_filepath) - ) + "Error: attempting to add image {0} twice.".format(image_filepath)) all_mappings[image_filepath] = (current_label, previous_state) @@ -160,7 +172,7 @@ def generatorForH5py(data_mappings, chunk_size=32): """ for chunk_id in range(0, len(data_mappings), chunk_size): # Data is expected to be a dict of - data_chunk = data_mappings[chunk_id : chunk_id + chunk_size] + data_chunk = data_mappings[chunk_id: chunk_id + chunk_size] if len(data_chunk) == chunk_size: image_names_chunk = [a for (a, b) in data_chunk] labels_chunk = np.asarray([b[0] for (a, b) in data_chunk]) @@ -249,8 +261,12 @@ def cook(folders, output_directory, train_eval_test_split, chunk_size): train_eval_test_split: dataset split ratio """ output_files = [ - os.path.join(output_directory, f) for f in ["train.h5", "eval.h5", "test.h5"] - ] + os.path.join( + output_directory, + f) for f in [ + "train.h5", + "eval.h5", + "test.h5"]] if any([os.path.isfile(f) for f in output_files]): print( "Preprocessed data already exists at: {0}. Skipping preprocessing.".format( @@ -279,7 +295,8 @@ def preprocess_data( # chunk size for training batches chunk_size = 32 - # no test set needed, since testing in our case is running the model on an unseen map in AutonomySim + # no test set needed, since testing in our case is running the model on an + # unseen map in AutonomySim train_eval_test_split = [0.8, 0.2, 0.0] data_folders = [] diff --git a/python/autonomysim/ai/imitation/trainers.py b/python/autonomysim/ai/imitation/trainers.py index 9d7e2174..3b18ccba 100644 --- a/python/autonomysim/ai/imitation/trainers.py +++ b/python/autonomysim/ai/imitation/trainers.py @@ -1,16 +1,14 @@ +from autonomysim.ai.imitation.generators import DriveDataGenerator +from keras_tqdm import TQDMNotebookCallback +from keras.callbacks import ReduceLROnPlateau, ModelCheckpoint, CSVLogger, EarlyStopping +from keras.optimizers import Adam +from keras.layers import Conv2D, Dropout, Flatten, Dense, Input +from keras.models import Model +import h5py import os os.environ["TF_CPP_MIN_LOG_LEVEL"] = "2" -import h5py -from keras.models import Model -from keras.layers import Conv2D, Dropout, Flatten, Dense, Input -from keras.optimizers import Adam -from keras.callbacks import ReduceLROnPlateau, ModelCheckpoint, CSVLogger, EarlyStopping -from keras_tqdm import TQDMNotebookCallback - -from autonomysim.ai.imitation.generators import DriveDataGenerator - # << The directory containing the cooked data from the previous step >> COOKED_DATA_DIR = "./cooked_data/" @@ -19,7 +17,9 @@ MODEL_OUTPUT_DIR = "./models/" -def train_drive_model(input_dir=COOKED_DATA_DIR, output_dir=MODEL_OUTPUT_DIR) -> None: +def train_drive_model( + input_dir=COOKED_DATA_DIR, + output_dir=MODEL_OUTPUT_DIR) -> None: # Hyper-parameters batch_size = 32 learning_rate = 0.0001 @@ -29,7 +29,8 @@ def train_drive_model(input_dir=COOKED_DATA_DIR, output_dir=MODEL_OUTPUT_DIR) -> activation = "relu" out_activation = "sigmoid" - # Stop training if in the last 20 epochs, there was no change of the best recorded validation loss + # Stop training if in the last 20 epochs, there was no change of the best + # recorded validation loss training_patience = 20 train_dataset = h5py.File(os.path.join(input_dir, "train.h5"), "r") @@ -125,10 +126,17 @@ def train_drive_model(input_dir=COOKED_DATA_DIR, output_dir=MODEL_OUTPUT_DIR) -> 10, name="fc4", activation=activation, kernel_initializer="he_normal" )(img_stack) img_stack = Dense( - 1, name="output", activation=out_activation, kernel_initializer="he_normal" - )(img_stack) - - adam = Adam(lr=learning_rate, beta_1=0.9, beta_2=0.999, epsilon=1e-08, decay=0.0) + 1, + name="output", + activation=out_activation, + kernel_initializer="he_normal")(img_stack) + + adam = Adam( + lr=learning_rate, + beta_1=0.9, + beta_2=0.999, + epsilon=1e-08, + decay=0.0) model = Model(inputs=[pic_input], outputs=img_stack) model.compile(optimizer=adam, loss="mse") @@ -136,8 +144,11 @@ def train_drive_model(input_dir=COOKED_DATA_DIR, output_dir=MODEL_OUTPUT_DIR) -> model.summary() plateau_callback = ReduceLROnPlateau( - monitor="val_loss", factor=0.5, patience=3, min_lr=learning_rate, verbose=1 - ) + monitor="val_loss", + factor=0.5, + patience=3, + min_lr=learning_rate, + verbose=1) csv_callback = CSVLogger(os.path.join(output_dir, "training_log.csv")) checkpoint_filepath = os.path.join( output_dir, diff --git a/python/autonomysim/ai/vision/benchmarks.py b/python/autonomysim/ai/vision/benchmarks.py index 119e6124..4378bab8 100644 --- a/python/autonomysim/ai/vision/benchmarks.py +++ b/python/autonomysim/ai/vision/benchmarks.py @@ -34,7 +34,10 @@ def saveImage(response, filename): cv2.imwrite(os.path.normpath(filename + ".png"), depth) elif response.compress: # png format - write_file(os.path.normpath(filename + ".png"), response.image_data_uint8) + write_file( + os.path.normpath( + filename + ".png"), + response.image_data_uint8) else: # uncompressed array img1d = np.fromstring( @@ -43,7 +46,10 @@ def saveImage(response, filename): img_rgb = img1d.reshape( response.height, response.width, 3 ) # reshape array to 3 channel image array H X W X 3 - cv2.imwrite(os.path.normpath(filename + ".png"), img_rgb) # write to png + cv2.imwrite( + os.path.normpath( + filename + ".png"), + img_rgb) # write to png class ImageBenchmarker: @@ -78,7 +84,8 @@ def __init__( self.is_image_thread_active = False if self.save_images: - self.tmp_dir = os.path.join(tempfile.gettempdir(), "autonomysim_img_bm") + self.tmp_dir = os.path.join( + tempfile.gettempdir(), "autonomysim_img_bm") print(f"Saving images to {self.tmp_dir}") try: os.makedirs(self.tmp_dir) @@ -98,7 +105,8 @@ def stop_img_benchmark_thread(self): self.is_image_thread_active = False self.image_callback_thread.join() print("Stopped image callback thread.") - print(f"FPS: {self.avg_fps} for {self.image_benchmark_num_images} images") + print( + f"FPS: {self.avg_fps} for {self.image_benchmark_num_images} images") def repeat_timer_img(self, task, period): while self.is_image_thread_active: @@ -138,13 +146,11 @@ def image_callback_benchmark_simGetImages(self): if response.pixels_as_float: print( f"Type {response.image_type}, size {len(response.image_data_float)}," - f"height {response.height}, width {response.width}" - ) + f"height {response.height}, width {response.width}") else: print( f"Type {response.image_type}, size {len(response.image_data_uint8)}," - f"height {response.height}, width {response.width}" - ) + f"height {response.height}, width {response.width}") if self.viz_image_cv2: np_arr = np.frombuffer(response.image_data_uint8, dtype=np.uint8) @@ -153,7 +159,9 @@ def image_callback_benchmark_simGetImages(self): cv2.waitKey(1) if self.save_images: - filename = os.path.join(self.tmp_dir, str(self.image_benchmark_num_images)) + filename = os.path.join( + self.tmp_dir, str( + self.image_benchmark_num_images)) saveImage(response, filename) @@ -192,8 +200,10 @@ def main(args): "--img_type", type=str, choices=cameraTypeMap.keys(), default="scene" ) parser.add_argument( - "--time", help="Time in secs to run the benchmark for", type=int, default=30 - ) + "--time", + help="Time in secs to run the benchmark for", + type=int, + default=30) args = parser.parse_args() main(args) diff --git a/python/autonomysim/ai/vision/navigation.py b/python/autonomysim/ai/vision/navigation.py index 367df083..5624aa58 100644 --- a/python/autonomysim/ai/vision/navigation.py +++ b/python/autonomysim/ai/vision/navigation.py @@ -23,7 +23,13 @@ def get_next_vec(self, depth, obj_sz, goal, pos): class AvoidLeft(AbstractClassGetNextVec): - def __init__(self, hfov=radians(90), coll_thres=5, yaw=0, limit_yaw=5, step=0.1): + def __init__( + self, + hfov=radians(90), + coll_thres=5, + yaw=0, + limit_yaw=5, + step=0.1): self.hfov = hfov self.coll_thres = coll_thres self.yaw = yaw @@ -39,8 +45,8 @@ def get_next_vec(self, depth, obj_sz, goal, pos): # compute box of interest img2d_box = img2d[ - int((h - roi_h) / 2) : int((h + roi_h) / 2), - int((w - roi_w) / 2) : int((w + roi_w) / 2), + int((h - roi_h) / 2): int((h + roi_h) / 2), + int((w - roi_w) / 2): int((w + roi_w) / 2), ] # scale by weight matrix (optional) @@ -50,7 +56,8 @@ def get_next_vec(self, depth, obj_sz, goal, pos): if np.min(img2d_box) < coll_thres: self.yaw = self.yaw - radians(self.limit_yaw) else: - self.yaw = self.yaw + min(t_angle - self.yaw, radians(self.limit_yaw)) + self.yaw = self.yaw + \ + min(t_angle - self.yaw, radians(self.limit_yaw)) pos[0] = pos[0] + self.step * np.cos(self.yaw) pos[1] = pos[1] + self.step * np.sin(self.yaw) @@ -59,7 +66,13 @@ def get_next_vec(self, depth, obj_sz, goal, pos): class AvoidLeftIgonreGoal(AbstractClassGetNextVec): - def __init__(self, hfov=radians(90), coll_thres=5, yaw=0, limit_yaw=5, step=0.1): + def __init__( + self, + hfov=radians(90), + coll_thres=5, + yaw=0, + limit_yaw=5, + step=0.1): self.hfov = hfov self.coll_thres = coll_thres self.yaw = yaw @@ -72,8 +85,8 @@ def get_next_vec(self, depth, obj_sz, goal, pos): # compute box of interest img2d_box = img2d[ - int((h - roi_h) / 2) : int((h + roi_h) / 2), - int((w - roi_w) / 2) : int((w + roi_w) / 2), + int((h - roi_h) / 2): int((h + roi_h) / 2), + int((w - roi_w) / 2): int((w + roi_w) / 2), ] # detect collision @@ -89,7 +102,8 @@ def get_next_vec(self, depth, obj_sz, goal, pos): class AvoidLeftRight(AbstractClassGetNextVec): def get_next_vec(self, depth, obj_sz, goal, pos): print("Some implementation!") - # Same as above but decide to go left or right based on average or some metric like that + # Same as above but decide to go left or right based on average or some + # metric like that return @@ -112,8 +126,10 @@ def get_local_goal(v, pos, theta): # compute bounding box size def compute_bb(image_sz, obj_sz, hfov, distance): vfov = hfov2vfov(hfov, image_sz) - box_h = np.ceil(obj_sz[0] * image_sz[0] / (math.tan(hfov / 2) * distance * 2)) - box_w = np.ceil(obj_sz[1] * image_sz[1] / (math.tan(vfov / 2) * distance * 2)) + box_h = np.ceil(obj_sz[0] * image_sz[0] / + (math.tan(hfov / 2) * distance * 2)) + box_w = np.ceil(obj_sz[1] * image_sz[1] / + (math.tan(vfov / 2) * distance * 2)) return box_h, box_w @@ -129,21 +145,23 @@ def equal_weight_mtx(roi_h, roi_w): return np.ones((roi_h, roi_w)) -# matrix with max weight in center and decreasing linearly with distance from center +# matrix with max weight in center and decreasing linearly with distance +# from center def linear_weight_mtx(roi_h, roi_w): w_mtx = np.ones((roi_h, roi_w)) for j in range(0, roi_w): for i in range(j, roi_h - j): - w_mtx[j : roi_h - j, i : roi_w - i] = j + 1 + w_mtx[j: roi_h - j, i: roi_w - i] = j + 1 return w_mtx -# matrix with max weight in center and decreasing quadratically with distance from center +# matrix with max weight in center and decreasing quadratically with +# distance from center def square_weight_mtx(roi_h, roi_w): w_mtx = np.ones((roi_h, roi_w)) for j in range(0, roi_w): for i in range(j, roi_h - j): - w_mtx[j : roi_h - j, i : roi_w - i] = (j + 1) * (j + 1) + w_mtx[j: roi_h - j, i: roi_w - i] = (j + 1) * (j + 1) return w_mtx diff --git a/python/autonomysim/clients.py b/python/autonomysim/clients.py index 4ab80b55..b91bccd6 100644 --- a/python/autonomysim/clients.py +++ b/python/autonomysim/clients.py @@ -20,7 +20,7 @@ def __init__(self, ip="", port=41451, timeout_value=3600): unpack_encoding="utf-8", ) - # ----------------------------------- Common vehicle APIs --------------------------------------------- + # ----------------------------------- Common vehicle APIs ---------------- def reset(self): """ Reset the vehicle to its original starting state @@ -134,7 +134,10 @@ def getHomeGeoPoint(self, vehicle_name=""): Returns: GeoPoint: Home location of the vehicle """ - return GeoPoint.from_msgpack(self.client.call("getHomeGeoPoint", vehicle_name)) + return GeoPoint.from_msgpack( + self.client.call( + "getHomeGeoPoint", + vehicle_name)) def confirmConnection(self): """ @@ -240,8 +243,10 @@ def simSetObjectMaterialFromTexture( bool: True if material was set """ return self.client.call( - "simSetObjectMaterialFromTexture", object_name, texture_path, component_id - ) + "simSetObjectMaterialFromTexture", + object_name, + texture_path, + component_id) # time-of-day control # time - of - day control @@ -303,7 +308,12 @@ def simSetWeatherParameter(self, param, val): # camera control # simGetImage returns compressed png in array of bytes # image_type uses one of the ImageType members - def simGetImage(self, camera_name, image_type, vehicle_name="", external=False): + def simGetImage( + self, + camera_name, + image_type, + vehicle_name="", + external=False): """ Get a single image @@ -320,10 +330,12 @@ def simGetImage(self, camera_name, image_type, vehicle_name="", external=False): Returns: Binary string literal of compressed png image """ - # todo : in future remove below, it's only for compatibility to pre v1.2 + # todo : in future remove below, it's only for compatibility to pre + # v1.2 camera_name = str(camera_name) - # because this method returns std::vector < uint8>, msgpack decides to encode it as a string unfortunately. + # because this method returns std::vector < uint8>, msgpack decides to + # encode it as a string unfortunately. result = self.client.call( "simGetImage", camera_name, image_type, vehicle_name, external ) @@ -351,12 +363,15 @@ def simGetImages(self, requests, vehicle_name="", external=False): responses_raw = self.client.call( "simGetImages", requests, vehicle_name, external ) - return [ - ImageResponse.from_msgpack(response_raw) for response_raw in responses_raw - ] + return [ImageResponse.from_msgpack( + response_raw) for response_raw in responses_raw] # CinemAutonomySim - def simGetPresetLensSettings(self, camera_name, vehicle_name="", external=False): + def simGetPresetLensSettings( + self, + camera_name, + vehicle_name="", + external=False): result = self.client.call( "simGetPresetLensSettings", camera_name, vehicle_name, external ) @@ -373,8 +388,11 @@ def simGetLensSettings(self, camera_name, vehicle_name="", external=False): return result def simSetPresetLensSettings( - self, preset_lens_settings, camera_name, vehicle_name="", external=False - ): + self, + preset_lens_settings, + camera_name, + vehicle_name="", + external=False): self.client.call( "simSetPresetLensSettings", preset_lens_settings, @@ -394,8 +412,11 @@ def simGetPresetFilmbackSettings( return result def simSetPresetFilmbackSettings( - self, preset_filmback_settings, camera_name, vehicle_name="", external=False - ): + self, + preset_filmback_settings, + camera_name, + vehicle_name="", + external=False): self.client.call( "simSetPresetFilmbackSettings", preset_filmback_settings, @@ -404,7 +425,11 @@ def simSetPresetFilmbackSettings( external, ) - def simGetFilmbackSettings(self, camera_name, vehicle_name="", external=False): + def simGetFilmbackSettings( + self, + camera_name, + vehicle_name="", + external=False): result = self.client.call( "simGetFilmbackSettings", camera_name, vehicle_name, external ) @@ -413,8 +438,12 @@ def simGetFilmbackSettings(self, camera_name, vehicle_name="", external=False): return result def simSetFilmbackSettings( - self, sensor_width, sensor_height, camera_name, vehicle_name="", external=False - ): + self, + sensor_width, + sensor_height, + camera_name, + vehicle_name="", + external=False): return self.client.call( "simSetFilmbackSettings", sensor_width, @@ -433,8 +462,11 @@ def simSetFocalLength( self, focal_length, camera_name, vehicle_name="", external=False ): self.client.call( - "simSetFocalLength", focal_length, camera_name, vehicle_name, external - ) + "simSetFocalLength", + focal_length, + camera_name, + vehicle_name, + external) def simEnableManualFocus( self, enable, camera_name, vehicle_name="", external=False @@ -443,7 +475,11 @@ def simEnableManualFocus( "simEnableManualFocus", enable, camera_name, vehicle_name, external ) - def simGetFocusDistance(self, camera_name, vehicle_name="", external=False): + def simGetFocusDistance( + self, + camera_name, + vehicle_name="", + external=False): return self.client.call( "simGetFocusDistance", camera_name, vehicle_name, external ) @@ -452,10 +488,17 @@ def simSetFocusDistance( self, focus_distance, camera_name, vehicle_name="", external=False ): self.client.call( - "simSetFocusDistance", focus_distance, camera_name, vehicle_name, external - ) + "simSetFocusDistance", + focus_distance, + camera_name, + vehicle_name, + external) - def simGetFocusAperture(self, camera_name, vehicle_name="", external=False): + def simGetFocusAperture( + self, + camera_name, + vehicle_name="", + external=False): return self.client.call( "simGetFocusAperture", camera_name, vehicle_name, external ) @@ -464,15 +507,27 @@ def simSetFocusAperture( self, focus_aperture, camera_name, vehicle_name="", external=False ): self.client.call( - "simSetFocusAperture", focus_aperture, camera_name, vehicle_name, external - ) + "simSetFocusAperture", + focus_aperture, + camera_name, + vehicle_name, + external) - def simEnableFocusPlane(self, enable, camera_name, vehicle_name="", external=False): + def simEnableFocusPlane( + self, + enable, + camera_name, + vehicle_name="", + external=False): self.client.call( "simEnableFocusPlane", enable, camera_name, vehicle_name, external ) - def simGetCurrentFieldOfView(self, camera_name, vehicle_name="", external=False): + def simGetCurrentFieldOfView( + self, + camera_name, + vehicle_name="", + external=False): return self.client.call( "simGetCurrentFieldOfView", camera_name, vehicle_name, external ) @@ -489,7 +544,10 @@ def simTestLineOfSightToPoint(self, point, vehicle_name=""): Returns: [bool]: Success """ - return self.client.call("simTestLineOfSightToPoint", point, vehicle_name) + return self.client.call( + "simTestLineOfSightToPoint", + point, + vehicle_name) def simTestLineOfSightBetweenPoints(self, point1, point2): """ @@ -502,7 +560,8 @@ def simTestLineOfSightBetweenPoints(self, point1, point2): Returns: [bool]: Success """ - return self.client.call("simTestLineOfSightBetweenPoints", point1, point2) + return self.client.call( + "simTestLineOfSightBetweenPoints", point1, point2) def simGetWorldExtents(self): """ @@ -512,7 +571,8 @@ def simGetWorldExtents(self): list[GeoPoint] """ responses_raw = self.client.call("simGetWorldExtents") - return [GeoPoint.from_msgpack(response_raw) for response_raw in responses_raw] + return [GeoPoint.from_msgpack(response_raw) + for response_raw in responses_raw] def simRunConsoleCommand(self, command): """ @@ -567,7 +627,11 @@ def simSetVehiclePose(self, pose, ignore_collision, vehicle_name=""): ignore_collision (bool): Whether to ignore any collision or not vehicle_name (str, optional): Name of the vehicle to move """ - self.client.call("simSetVehiclePose", pose, ignore_collision, vehicle_name) + self.client.call( + "simSetVehiclePose", + pose, + ignore_collision, + vehicle_name) def simGetVehiclePose(self, vehicle_name=""): """ @@ -593,7 +657,11 @@ def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name=""): thickness (float, optional): Thickness of the line vehicle_name (string, optional): Name of the vehicle to set Trace line values for """ - self.client.call("simSetTraceLine", color_rgba, thickness, vehicle_name) + self.client.call( + "simSetTraceLine", + color_rgba, + thickness, + vehicle_name) def simGetObjectPose(self, object_name): """ @@ -623,7 +691,11 @@ def simSetObjectPose(self, object_name, pose, teleport=True): Returns: bool: If the move was successful """ - return self.client.call("simSetObjectPose", object_name, pose, teleport) + return self.client.call( + "simSetObjectPose", + object_name, + pose, + teleport) def simGetObjectScale(self, object_name): """ @@ -729,7 +801,11 @@ def simDestroyObject(self, object_name): """ return self.client.call("simDestroyObject", object_name) - def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex=False): + def simSetSegmentationObjectID( + self, + mesh_name, + object_id, + is_name_regex=False): """ Set segmentation ID for specific objects @@ -761,8 +837,12 @@ def simGetSegmentationObjectID(self, mesh_name): return self.client.call("simGetSegmentationObjectID", mesh_name) def simAddDetectionFilterMeshName( - self, camera_name, image_type, mesh_name, vehicle_name="", external=False - ): + self, + camera_name, + image_type, + mesh_name, + vehicle_name="", + external=False): """ Add mesh name to detect in wild card format @@ -786,8 +866,12 @@ def simAddDetectionFilterMeshName( ) def simSetDetectionFilterRadius( - self, camera_name, image_type, radius_cm, vehicle_name="", external=False - ): + self, + camera_name, + image_type, + radius_cm, + vehicle_name="", + external=False): """ Set detection radius for all cameras @@ -846,9 +930,8 @@ def simGetDetections( responses_raw = self.client.call( "simGetDetections", camera_name, image_type, vehicle_name, external ) - return [ - DetectionInfo.from_msgpack(response_raw) for response_raw in responses_raw - ] + return [DetectionInfo.from_msgpack( + response_raw) for response_raw in responses_raw] def simPrintLogMessage(self, message, message_param="", severity=0): """ @@ -865,7 +948,11 @@ def simPrintLogMessage(self, message, message_param="", severity=0): message_param (str, optional): Parameter to be printed next to the message severity (int, optional): Range 0-3, inclusive, corresponding to the severity of the message """ - self.client.call("simPrintLogMessage", message, message_param, severity) + self.client.call( + "simPrintLogMessage", + message, + message_param, + severity) def simGetCameraInfo(self, camera_name, vehicle_name="", external=False): """ @@ -879,14 +966,19 @@ def simGetCameraInfo(self, camera_name, vehicle_name="", external=False): Returns: CameraInfo: """ - # TODO : below str() conversion is only needed for legacy reason and should be removed in future + # TODO : below str() conversion is only needed for legacy reason and + # should be removed in future return CameraInfo.from_msgpack( self.client.call( "simGetCameraInfo", str(camera_name), vehicle_name, external ) ) - def simGetDistortionParams(self, camera_name, vehicle_name="", external=False): + def simGetDistortionParams( + self, + camera_name, + vehicle_name="", + external=False): """ Get camera distortion parameters @@ -944,7 +1036,12 @@ def simSetDistortionParam( external, ) - def simSetCameraPose(self, camera_name, pose, vehicle_name="", external=False): + def simSetCameraPose( + self, + camera_name, + pose, + vehicle_name="", + external=False): """ - Control the pose of a selected camera @@ -954,7 +1051,8 @@ def simSetCameraPose(self, camera_name, pose, vehicle_name="", external=False): vehicle_name (str, optional): Name of vehicle which the camera corresponds to external (bool, optional): Whether the camera is an External Camera """ - # TODO : below str() conversion is only needed for legacy reason and should be removed in future + # TODO : below str() conversion is only needed for legacy reason and + # should be removed in future self.client.call( "simSetCameraPose", str(camera_name), pose, vehicle_name, external ) @@ -971,10 +1069,14 @@ def simSetCameraFov( vehicle_name (str, optional): Name of vehicle which the camera corresponds to external (bool, optional): Whether the camera is an External Camera """ - # TODO : below str() conversion is only needed for legacy reason and should be removed in future + # TODO : below str() conversion is only needed for legacy reason and + # should be removed in future self.client.call( - "simSetCameraFov", str(camera_name), fov_degrees, vehicle_name, external - ) + "simSetCameraFov", + str(camera_name), + fov_degrees, + vehicle_name, + external) def simGetGroundTruthKinematics(self, vehicle_name=""): """ @@ -988,7 +1090,8 @@ def simGetGroundTruthKinematics(self, vehicle_name=""): Returns: KinematicsState: Ground truth of the vehicle """ - kinematics_state = self.client.call("simGetGroundTruthKinematics", vehicle_name) + kinematics_state = self.client.call( + "simGetGroundTruthKinematics", vehicle_name) return KinematicsState.from_msgpack(kinematics_state) simGetGroundTruthKinematics.__annotations__ = {"return": KinematicsState} @@ -1004,7 +1107,11 @@ def simSetKinematics(self, state, ignore_collision, vehicle_name=""): ignore_collision (bool): Whether to ignore any collision or not vehicle_name (str, optional): Name of the vehicle to move """ - self.client.call("simSetKinematics", state, ignore_collision, vehicle_name) + self.client.call( + "simSetKinematics", + state, + ignore_collision, + vehicle_name) def simGetGroundTruthEnvironment(self, vehicle_name=""): """ @@ -1018,7 +1125,8 @@ def simGetGroundTruthEnvironment(self, vehicle_name=""): Returns: EnvironmentState: Ground truth environment state """ - env_state = self.client.call("simGetGroundTruthEnvironment", vehicle_name) + env_state = self.client.call( + "simGetGroundTruthEnvironment", vehicle_name) return EnvironmentState.from_msgpack(env_state) simGetGroundTruthEnvironment.__annotations__ = {"return": EnvironmentState} @@ -1060,8 +1168,10 @@ def getMagnetometerData(self, magnetometer_name="", vehicle_name=""): MagnetometerData: """ return MagnetometerData.from_msgpack( - self.client.call("getMagnetometerData", magnetometer_name, vehicle_name) - ) + self.client.call( + "getMagnetometerData", + magnetometer_name, + vehicle_name)) def getGpsData(self, gps_name="", vehicle_name=""): """ @@ -1169,8 +1279,12 @@ def simPlotLineStrip( is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ self.client.call( - "simPlotLineStrip", points, color_rgba, thickness, duration, is_persistent - ) + "simPlotLineStrip", + points, + color_rgba, + thickness, + duration, + is_persistent) def simPlotLineList( self, @@ -1191,8 +1305,12 @@ def simPlotLineList( is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ self.client.call( - "simPlotLineList", points, color_rgba, thickness, duration, is_persistent - ) + "simPlotLineList", + points, + color_rgba, + thickness, + duration, + is_persistent) def simPlotArrows( self, @@ -1250,8 +1368,12 @@ def simPlotStrings( ) def simPlotTransforms( - self, poses, scale=5.0, thickness=5.0, duration=-1.0, is_persistent=False - ): + self, + poses, + scale=5.0, + thickness=5.0, + duration=-1.0, + is_persistent=False): """ Plots a list of transforms in World NED frame. @@ -1263,8 +1385,12 @@ def simPlotTransforms( is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. """ self.client.call( - "simPlotTransforms", poses, scale, thickness, duration, is_persistent - ) + "simPlotTransforms", + poses, + scale, + thickness, + duration, + is_persistent) def simPlotTransformsWithNames( self, @@ -1354,7 +1480,8 @@ def simCreateVoxelGrid(self, position, x, y, z, res, of): Returns: bool: True if output written to file successfully, else False """ - return self.client.call("simCreateVoxelGrid", position, x, y, z, res, of) + return self.client.call( + "simCreateVoxelGrid", position, x, y, z, res, of) # Add new vehicle via RPC def simAddVehicle(self, vehicle_name, vehicle_type, pose, pawn_path=""): @@ -1403,7 +1530,7 @@ def simSetExtForce(self, ext_force): self.client.call("simSetExtForce", ext_force) -# ----------------------------------- Multirotor APIs --------------------------------------------- +# ----------------------------------- Multirotor APIs ------------------- class MultirotorClient(VehicleClient, object): def __init__(self, ip="", port=41451, timeout_value=3600): super(MultirotorClient, self).__init__(ip, port, timeout_value) @@ -1517,13 +1644,19 @@ def moveByVelocityZBodyFrameAsync( vehicle_name, ) - def moveByAngleZAsync(self, pitch, roll, z, yaw, duration, vehicle_name=""): + def moveByAngleZAsync( + self, + pitch, + roll, + z, + yaw, + duration, + vehicle_name=""): logging.warning( "moveByAngleZAsync API is deprecated, use moveByRollPitchYawZAsync() API instead" ) return self.client.call_async( - "moveByRollPitchYawZ", roll, -pitch, -yaw, z, duration, vehicle_name - ) + "moveByRollPitchYawZ", roll, -pitch, -yaw, z, duration, vehicle_name) def moveByAngleThrottleAsync( self, pitch, roll, throttle, yaw_rate, duration, vehicle_name="" @@ -1565,8 +1698,14 @@ def moveByVelocityAsync( msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ return self.client.call_async( - "moveByVelocity", vx, vy, vz, duration, drivetrain, yaw_mode, vehicle_name - ) + "moveByVelocity", + vx, + vy, + vz, + duration, + drivetrain, + yaw_mode, + vehicle_name) def moveByVelocityZAsync( self, @@ -1579,8 +1718,14 @@ def moveByVelocityZAsync( vehicle_name="", ): return self.client.call_async( - "moveByVelocityZ", vx, vy, z, duration, drivetrain, yaw_mode, vehicle_name - ) + "moveByVelocityZ", + vx, + vy, + z, + duration, + drivetrain, + yaw_mode, + vehicle_name) def moveOnPathAsync( self, @@ -1719,7 +1864,12 @@ def moveByManualAsync( vehicle_name, ) - def rotateToYawAsync(self, yaw, timeout_sec=3e38, margin=5, vehicle_name=""): + def rotateToYawAsync( + self, + yaw, + timeout_sec=3e38, + margin=5, + vehicle_name=""): return self.client.call_async( "rotateToYaw", yaw, timeout_sec, margin, vehicle_name ) @@ -1768,7 +1918,14 @@ def moveByMotorPWMsAsync( vehicle_name, ) - def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name=""): + def moveByRollPitchYawZAsync( + self, + roll, + pitch, + yaw, + z, + duration, + vehicle_name=""): """ - z is given in local NED frame of the vehicle. - Roll angle, pitch angle, and yaw angle set points are given in **radians**, in the body frame. @@ -1802,8 +1959,7 @@ def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name=" msgpackrpc.future.Future: future. call .join() to wait for method to finish. Example: client.METHOD().join() """ return self.client.call_async( - "moveByRollPitchYawZ", roll, -pitch, -yaw, z, duration, vehicle_name - ) + "moveByRollPitchYawZ", roll, -pitch, -yaw, z, duration, vehicle_name) def moveByRollPitchYawThrottleAsync( self, roll, pitch, yaw, throttle, duration, vehicle_name="" @@ -1986,8 +2142,13 @@ def moveByAngleRatesZAsync( ) def moveByAngleRatesThrottleAsync( - self, roll_rate, pitch_rate, yaw_rate, throttle, duration, vehicle_name="" - ): + self, + roll_rate, + pitch_rate, + yaw_rate, + throttle, + duration, + vehicle_name=""): """ - Desired throttle is between 0.0 to 1.0 - Roll rate, pitch rate, and yaw rate set points are given in **radians**, in the body frame. @@ -2086,9 +2247,8 @@ def setVelocityControllerGains( - Modifying velocity controller gains will have an affect on the behaviour of moveOnSplineAsync() and moveOnSplineVelConstraintsAsync(), as they both use velocity control to track the trajectory. vehicle_name='' (str, optional): Name of the multirotor to send this command to """ - self.client.call( - "setVelocityControllerGains", *(velocity_gains.to_lists() + (vehicle_name,)) - ) + self.client.call("setVelocityControllerGains", * + (velocity_gains.to_lists() + (vehicle_name,))) def setPositionControllerGains( self, position_gains=PositionControllerGains(), vehicle_name="" @@ -2103,9 +2263,8 @@ def setPositionControllerGains( - Pass PositionControllerGains() to reset gains to default recommended values. vehicle_name =''(str, optional): Name of the multirotor to send this command to """ - self.client.call( - "setPositionControllerGains", *(position_gains.to_lists() + (vehicle_name,)) - ) + self.client.call("setPositionControllerGains", * + (position_gains.to_lists() + (vehicle_name,))) # query vehicle state def getMultirotorState(self, vehicle_name=""): @@ -2143,7 +2302,7 @@ def getRotorStates(self, vehicle_name=""): getRotorStates.__annotations__ = {"return": RotorStates} -# ----------------------------------- Car APIs --------------------------------------------- +# ----------------------------------- Car APIs --------------------------- class CarClient(VehicleClient, object): def __init__(self, ip="", port=41451, timeout_value=3600): super(CarClient, self).__init__(ip, port, timeout_value) diff --git a/python/autonomysim/gym/envs/autonomysim_env.py b/python/autonomysim/gym/envs/autonomysim_env.py index 4a96a3e4..4bc56744 100644 --- a/python/autonomysim/gym/envs/autonomysim_env.py +++ b/python/autonomysim/gym/envs/autonomysim_env.py @@ -7,7 +7,8 @@ class AutonomySimEnv(gym.Env): metadata = {"render.modes": ["rgb_array"]} def __init__(self, image_shape): - self.observation_space = spaces.Box(0, 255, shape=image_shape, dtype=np.uint8) + self.observation_space = spaces.Box( + 0, 255, shape=image_shape, dtype=np.uint8) self.viewer = None def __del__(self): diff --git a/python/autonomysim/navigation/surveying.py b/python/autonomysim/navigation/surveying.py index e7399779..cf783c23 100644 --- a/python/autonomysim/navigation/surveying.py +++ b/python/autonomysim/navigation/surveying.py @@ -37,13 +37,15 @@ def start(self): self.client.moveToPositionAsync(0, 0, z, self.velocity).join() print("flying to first corner of survey box") - self.client.moveToPositionAsync(x, -self.boxsize, z, self.velocity).join() + self.client.moveToPositionAsync( + x, -self.boxsize, z, self.velocity).join() # let it settle there a bit. self.client.hoverAsync().join() time.sleep(2) - # after hovering we need to re-enabled api control for next leg of the trip + # after hovering we need to re-enabled api control for next leg of the + # trip self.client.enableApiControl(True) # now compute the survey path required to fill the box diff --git a/python/autonomysim/sensors/event_camera.py b/python/autonomysim/sensors/event_camera.py index ae4ee523..58875e58 100644 --- a/python/autonomysim/sensors/event_camera.py +++ b/python/autonomysim/sensors/event_camera.py @@ -5,9 +5,8 @@ from numba import njit, prange # set_num_threads -EVENT_TYPE = np.dtype( - [("timestamp", "f8"), ("x", "u2"), ("y", "u2"), ("polarity", "b")], align=True -) +EVENT_TYPE = np.dtype([("timestamp", "f8"), ("x", "u2"), + ("y", "u2"), ("polarity", "b")], align=True) TOL = 0.5 MINIMUM_CONTRAST_THRESHOLD = 0.01 @@ -96,7 +95,9 @@ def __init__(self, W, H, first_image=None, first_time=None, config=CONFIG): self.npix = H * W def init(self, first_image, first_time): - print("Initialized event camera simulator with sensor size:", first_image.shape) + print( + "Initialized event camera simulator with sensor size:", + first_image.shape) self.resolution = first_image.shape # The resolution of the image diff --git a/python/autonomysim/sensors/thermal_camera.py b/python/autonomysim/sensors/thermal_camera.py index 23d93f5e..4f50a5a9 100644 --- a/python/autonomysim/sensors/thermal_camera.py +++ b/python/autonomysim/sensors/thermal_camera.py @@ -149,7 +149,7 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): """ # First set everything to 0. - success = client.simSetSegmentationObjectID("[\w]*", 0, True) + success = client.simSetSegmentationObjectID("[\\w]*", 0, True) if not success: print("There was a problem setting all segmentation object IDs to 0. ") sys.exit(1) @@ -157,19 +157,17 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): # Next set all objects of interest provided to corresponding object IDs # segIdDict values MUST match tempEmissivityNew labels. for key in segIdDict: - objectID = int( - tempEmissivityNew[numpy.where(tempEmissivityNew == segIdDict[key])[0], 1][0] - ) + objectID = int(tempEmissivityNew[numpy.where( + tempEmissivityNew == segIdDict[key])[0], 1][0]) success = client.simSetSegmentationObjectID( - "[\w]*" + key + "[\w]*", objectID, True + "[\\w]*" + key + "[\\w]*", objectID, True ) if not success: print( "There was a problem setting {0} segmentation object ID to {1!s}, or no {0} was found.".format( - key, objectID - ) - ) + key, + objectID)) time.sleep(0.1) @@ -208,7 +206,8 @@ def project_3d_point_to_screen( # Create a rotation matrix from camera pitch, roll, and yaw angles. camRotation = rotation_matrix_from_angles(pitchRollYaw) - # Change coordinates to get subjectXYZ in the camera's local coordinate system. + # Change coordinates to get subjectXYZ in the camera's local coordinate + # system. XYZW = numpy.transpose([subjectXYZ]) XYZW = numpy.add(XYZW, -camPosition) print("XYZW: " + str(XYZW)) @@ -220,7 +219,8 @@ def project_3d_point_to_screen( XYZW = numpy.matmul(camProjMatrix4x4, XYZW) XYZW = XYZW / XYZW[3] - # Move origin to the upper-left corner of the screen and multiply by size to get pixel values. Note that screen is in y,-z plane. + # Move origin to the upper-left corner of the screen and multiply by size + # to get pixel values. Note that screen is in y,-z plane. normX = (1 - XYZW[0]) / 2 normY = (1 + XYZW[1]) / 2 @@ -271,7 +271,8 @@ def get_image(x, y, z, pitch, roll, yaw, client): Shital Shah """ - # Set pose and sleep after to ensure the pose sticks before capturing image. + # Set pose and sleep after to ensure the pose sticks before capturing + # image. client.simSetVehiclePose( Pose(Vector3r(x, y, z), to_quaternion(pitch, roll, yaw)), True ) @@ -290,7 +291,9 @@ def get_image(x, y, z, pitch, roll, yaw, client): img1d = numpy.fromstring(responses[0].image_data_uint8, dtype=numpy.uint8) im = img1d.reshape(responses[0].height, responses[0].width, 4) - img1dscene = numpy.fromstring(responses[1].image_data_uint8, dtype=numpy.uint8) + img1dscene = numpy.fromstring( + responses[1].image_data_uint8, + dtype=numpy.uint8) imScene = img1dscene.reshape(responses[1].height, responses[1].width, 4) return ( diff --git a/python/autonomysim/types.py b/python/autonomysim/types.py index d9ed97fc..45564a97 100644 --- a/python/autonomysim/types.py +++ b/python/autonomysim/types.py @@ -7,7 +7,8 @@ class MsgpackMixin: def __repr__(self): from pprint import pformat - return "<" + type(self).__name__ + "> " + pformat(vars(self), indent=4, width=1) + return "<" + type(self).__name__ + "> " + \ + pformat(vars(self), indent=4, width=1) def to_msgpack(self, *args, **kwargs): return self.__dict__ @@ -63,10 +64,9 @@ def OpticalFlowVis(cls): def __getattr__(self, key): if key == "DepthPlanner": print( - "\033[31m" - + "DepthPlanner has been (correctly) renamed to DepthPlanar. Please use ImageType.DepthPlanar instead." - + "\033[0m" - ) + "\033[31m" + + "DepthPlanner has been (correctly) renamed to DepthPlanar. Please use ImageType.DepthPlanar instead." + + "\033[0m") raise AttributeError @@ -130,18 +130,22 @@ def nanVector3r(): def containsNan(self): return ( - math.isnan(self.x_val) or math.isnan(self.y_val) or math.isnan(self.z_val) - ) + math.isnan( + self.x_val) or math.isnan( + self.y_val) or math.isnan( + self.z_val)) def __add__(self, other): return Vector3r( - self.x_val + other.x_val, self.y_val + other.y_val, self.z_val + other.z_val - ) + self.x_val + other.x_val, + self.y_val + other.y_val, + self.z_val + other.z_val) def __sub__(self, other): return Vector3r( - self.x_val - other.x_val, self.y_val - other.y_val, self.z_val - other.z_val - ) + self.x_val - other.x_val, + self.y_val - other.y_val, + self.z_val - other.z_val) def __truediv__(self, other): if ( @@ -151,7 +155,10 @@ def __truediv__(self, other): + np.sctypes["uint"] + np.sctypes["float"] ): - return Vector3r(self.x_val / other, self.y_val / other, self.z_val / other) + return Vector3r( + self.x_val / other, + self.y_val / other, + self.z_val / other) else: raise TypeError( "unsupported operand type(s) for /: %s and %s" @@ -166,7 +173,10 @@ def __mul__(self, other): + np.sctypes["uint"] + np.sctypes["float"] ): - return Vector3r(self.x_val * other, self.y_val * other, self.z_val * other) + return Vector3r( + self.x_val * other, + self.y_val * other, + self.z_val * other) else: raise TypeError( "unsupported operand type(s) for *: %s and %s" @@ -174,7 +184,7 @@ def __mul__(self, other): ) def dot(self, other): - if type(self) == type(other): + if isinstance(self, type(other)): return ( self.x_val * other.x_val + self.y_val * other.y_val @@ -187,9 +197,14 @@ def dot(self, other): ) def cross(self, other): - if type(self) == type(other): - cross_product = np.cross(self.to_numpy_array(), other.to_numpy_array()) - return Vector3r(cross_product[0], cross_product[1], cross_product[2]) + if isinstance(self, type(other)): + cross_product = np.cross( + self.to_numpy_array(), + other.to_numpy_array()) + return Vector3r( + cross_product[0], + cross_product[1], + cross_product[2]) else: raise TypeError( "unsupported operand type(s) for 'cross': %s and %s" @@ -241,7 +256,7 @@ def containsNan(self): ) def __add__(self, other): - if type(self) == type(other): + if isinstance(self, type(other)): return Quaternionr( self.x_val + other.x_val, self.y_val + other.y_val, @@ -255,7 +270,7 @@ def __add__(self, other): ) def __mul__(self, other): - if type(self) == type(other): + if isinstance(self, type(other)): t, x, y, z = self.w_val, self.x_val, self.y_val, self.z_val a, b, c, d = other.w_val, other.x_val, other.y_val, other.z_val return Quaternionr( @@ -271,7 +286,7 @@ def __mul__(self, other): ) def __truediv__(self, other): - if type(other) == type(self): + if isinstance(other, type(self)): return self * other.inverse() elif ( type(other) @@ -293,7 +308,7 @@ def __truediv__(self, other): ) def dot(self, other): - if type(self) == type(other): + if isinstance(self, type(other)): return ( self.x_val * other.x_val + self.y_val * other.y_val @@ -307,7 +322,7 @@ def dot(self, other): ) def cross(self, other): - if type(self) == type(other): + if isinstance(self, type(other)): return (self * other - other * self) / 2 else: raise TypeError( @@ -316,7 +331,7 @@ def cross(self, other): ) def outer_product(self, other): - if type(self) == type(other): + if isinstance(self, type(other)): return (self.inverse() * other - other.inverse() * self) / 2 else: raise TypeError( @@ -325,7 +340,7 @@ def outer_product(self, other): ) def rotate(self, other): - if type(self) == type(other): + if isinstance(self, type(other)): if other.get_length() == 1: return other * self * other.inverse() else: @@ -349,7 +364,11 @@ def sgn(self): return self / self.get_length() def get_length(self): - return (self.x_val**2 + self.y_val**2 + self.z_val**2 + self.w_val**2) ** 0.5 + return ( + self.x_val**2 + + self.y_val**2 + + self.z_val**2 + + self.w_val**2) ** 0.5 def to_numpy_array(self): return np.array( @@ -458,7 +477,12 @@ class ImageRequest(MsgpackMixin): pixels_as_float = False compress = False - def __init__(self, camera_name, image_type, pixels_as_float=False, compress=True): + def __init__( + self, + camera_name, + image_type, + pixels_as_float=False, + compress=True): # todo: in future remove str(), it's only for compatibility to pre v1.2 self.camera_name = str(camera_name) self.image_type = image_type diff --git a/python/autonomysim/utils/__init__.py b/python/autonomysim/utils/__init__.py index 49d68983..e21c5796 100644 --- a/python/autonomysim/utils/__init__.py +++ b/python/autonomysim/utils/__init__.py @@ -120,20 +120,20 @@ def generate_color_palette(numPixelsWide, outputFile): choice = 0 j = 0 for i in range(3): - palette[0, j * numPixelsWide : (j + 1) * numPixelsWide, i] = choice + palette[0, j * numPixelsWide: (j + 1) * numPixelsWide, i] = choice colors[j][i] = choice for i in range(3): for j in range(1, 255): choice = random.sample(possibilities[i], 1)[0] possibilities[i].remove(choice) - palette[0, j * numPixelsWide : (j + 1) * numPixelsWide, i] = choice + palette[0, j * numPixelsWide: (j + 1) * numPixelsWide, i] = choice colors[j][i] = choice choice = 255 j = 255 for i in range(3): - palette[0, j * numPixelsWide : (j + 1) * numPixelsWide, i] = choice + palette[0, j * numPixelsWide: (j + 1) * numPixelsWide, i] = choice colors[j][i] = choice cv2.imwrite(outputFile, palette, [cv2.IMWRITE_PNG_COMPRESSION, 0]) diff --git a/python/autonomysim/utils/io/__init__.py b/python/autonomysim/utils/io/__init__.py index 3961cb03..f6f46ed0 100644 --- a/python/autonomysim/utils/io/__init__.py +++ b/python/autonomysim/utils/io/__init__.py @@ -62,7 +62,8 @@ def read_pfm(file): if dim_match: width, height = map(int, dim_match.groups()) else: - raise Exception("Malformed PFM header: width, height cannot be found") + raise Exception( + "Malformed PFM header: width, height cannot be found") scale = float(file.readline().rstrip()) if scale < 0: # little-endian @@ -97,7 +98,8 @@ def write_pfm(file, image, scale=1): ): # greyscale color = False else: - raise Exception("Image must have H x W x 3, H x W x 1 or H x W dimensions.") + raise Exception( + "Image must have H x W x 3, H x W x 1 or H x W dimensions.") file.write(bytes("PF\n", "UTF-8") if color else bytes("Pf\n", "UTF-8")) temp_str = "%d %d\n" % (image.shape[1], image.shape[0]) diff --git a/python/autonomysim/utils/io/audio.py b/python/autonomysim/utils/io/audio.py index 3a43e5b5..d48751b1 100644 --- a/python/autonomysim/utils/io/audio.py +++ b/python/autonomysim/utils/io/audio.py @@ -54,15 +54,18 @@ def open(self, filename, buffer_size, speaker=None): self.actual_channels = self.wav_file.getnchannels() self.actual_rate = self.wav_file.getframerate() self.sample_width = self.wav_file.getsampwidth() - # assumes signed integer used in raw audio, so for example, the max for 16bit is 2^15 (32768) + # assumes signed integer used in raw audio, so for example, the max for + # 16bit is 2^15 (32768) if self.auto_scale: self.audio_scale_factor = 1 / pow(2, (8 * self.sample_width) - 1) if self.requested_rate == 0: raise Exception("Requested rate cannot be zero") self.buffer_size = int( - math.ceil((self.read_size * self.actual_rate) / self.requested_rate) - ) + math.ceil( + (self.read_size * + self.actual_rate) / + self.requested_rate)) # convert int16 data to scaled floats if self.sample_width == 1: @@ -78,7 +81,10 @@ def open(self, filename, buffer_size, speaker=None): if speaker: # configure output stream to match what we are resampling to... audio_format = self.audio.get_format_from_width(self.sample_width) - speaker.open(audio_format, self.requested_channels, self.requested_rate) + speaker.open( + audio_format, + self.requested_channels, + self.requested_rate) def read_raw(self): """Read the next chunk of audio data. @@ -121,9 +127,9 @@ def get_requested_channels(self, data): channels = [] # split into separate channels for i in range(self.actual_channels): - channels += [data[i :: self.actual_channels]] + channels += [data[i:: self.actual_channels]] # drop the channels we don't want - channels = channels[0 : self.requested_channels] + channels = channels[0: self.requested_channels] # zip the resulting channels back up. data = np.array(list(zip(*channels))).flatten() # convert back to packed bytes in PCM 16 format @@ -144,8 +150,8 @@ def read(self): # deal with any accumulation of tails, if the tail grows to a full # buffer then return it! if self.tail is not None and len(self.tail) >= self.read_size: - data = self.tail[0 : self.read_size] - self.tail = self.tail[self.read_size :] + data = self.tail[0: self.read_size] + self.tail = self.tail[self.read_size:] return data data = self.read_raw() @@ -161,11 +167,12 @@ def read(self): data = np.concatenate((self.tail, data)) # now the caller needs us to stick to our sample_size contract, but when - # rate conversion happens we can't be sure that 'data' is exactly that size. + # rate conversion happens we can't be sure that 'data' is exactly that + # size. if len(data) > self.read_size: # usually one byte extra so add this to our accumulating tail - self.tail = data[self.read_size :] - data = data[0 : self.read_size] + self.tail = data[self.read_size:] + data = data[0: self.read_size] if len(data) < self.read_size: # might have reached the end of a file, so pad with zeros. diff --git a/scripts/build.ps1 b/scripts/build.ps1 index 67fe49f4..20c57785 100644 --- a/scripts/build.ps1 +++ b/scripts/build.ps1 @@ -8,10 +8,12 @@ AUTHOR: DATE: 2024-02-19 PARAMETERS: - - BuildMode: [ Debug | Release | RelWithDebInfo ] - - BuildDocs: Enable to build and serve AutonomySim documentation. - - HighPolycountSuv: Enable for an Unreal Engine full-polycount SUV asset. - - SystemDebug: Enable for computer system debugging messages. + - BuildMode: [ Debug | Release | RelWithDebInfo ] + - CmakeGenerator: [ 'Visual Studio 17 2022' | 'Visual Studio 16 2019' ] + - BuildDocs: Enable to build and serve AutonomySim documentation. + - SystemDebug: Enable for computer system debugging messages. + - IntegrateDeploy: Enable to automate Visual Studio installation selection. + - AssetSuv: Enable for an Unreal Engine full-polycount SUV asset. NOTES: Assumes: PowerShell version >= 7, Unreal Engine >= 5, CMake >= 3.14, Visual Studio 2022. Script is intended to run from the 'AutonomySim' base project directory. @@ -27,6 +29,9 @@ param( [Parameter(HelpMessage = 'Options: [ Debug | Release | RelWithDebInfo ]')] [String] $BuildMode = 'Release', + [Parameter(HelpMessage = 'Options: [ "Visual Studio 17 2022" | "Visual Studio 16 2019" ]')] + [String] + $CmakeGenerator = 'Visual Studio 17 2022', [Parameter(HelpMessage = 'Enable to build and serve AutonomySim documentation.')] [Switch] $BuildDocs = $false, @@ -57,11 +62,11 @@ Import-Module "${PWD}\scripts\utils.psm1" # imports: Add-Directori Import-Module "${PWD}\scripts\build_docs.psm1" # imports: Build-Documentation # Tests -Import-Module "${PWD}\scripts\test_visualstudio.psm1" # imports: VS_VERSION_MINIMUM, Set-VsInstance, Get-VsInstanceVersion, Test-VisualStudioVersion -Import-Module "${PWD}\scripts\test_cmake.psm1" # imports: CMAKE_VERSION_MINIMUM, Test-CmakeVersion -Import-Module "${PWD}\scripts\test_rpclib.psm1" # imports: RPCLIB_VERSION, Test-RpcLibVersion -Import-Module "${PWD}\scripts\test_eigen.psm1" # imports: EIGEN_VERSION, Test-EigenVersion -Import-Module "${PWD}\scripts\test_unrealasset.psm1" # imports: ASSET_SUV_VERSION, Test-AssetSuvVersion +Import-Module "${PWD}\scripts\step_visualstudio.psm1" # imports: VS_VERSION_MINIMUM, Set-VsInstance, Get-VsInstanceVersion, Test-VisualStudioVersion +Import-Module "${PWD}\scripts\step_cmake.psm1" # imports: CMAKE_VERSION_MINIMUM, Test-CmakeVersion +Import-Module "${PWD}\scripts\step_rpclib.psm1" # imports: RPCLIB_VERSION, Test-RpcLibVersion +Import-Module "${PWD}\scripts\step_eigen.psm1" # imports: EIGEN_VERSION, Test-EigenVersion +Import-Module "${PWD}\scripts\step_unrealasset.psm1" # imports: ASSET_SUV_VERSION, Test-AssetSuvVersion ### ### Variables @@ -69,25 +74,25 @@ Import-Module "${PWD}\scripts\test_unrealasset.psm1" # imports: ASSET_SUV_VER # Static variables $PROJECT_DIR = "$PWD" -$SCRIPT_DIR = "${PROJECT_DIR}\scripts" +$SCRIPT_DIR = "${PROJECT_DIR}\scripts" # Command-line arguments $BUILD_MODE = "$BuildMode" $BUILD_DOCS = if ($BuildDocs) { $true } else { $false } $DEBUG_MODE = if ($SystemDebug) { $true } else { $false } $CI_CD_MODE = if ($IntegrateDeploy -eq $true) { $true } else { $false } -$ASSET_SUV = if ($AssetSuv) { $true } else { $false } +$ASSET_SUV = if ($AssetSuv) { $true } else { $false } # Dynamic variables -$SYSTEM_INFO = Get-ComputerInfo # WARNING: Windows only -$SYSTEM_PROCESSOR = "${env:PROCESSOR_IDENTIFIER}" +$SYSTEM_INFO = Get-ComputerInfo # WARNING: Windows only +$SYSTEM_PROCESSOR = "${env:PROCESSOR_IDENTIFIER}" $SYSTEM_ARCHITECTURE = "${env:PROCESSOR_ARCHITECTURE}" -$SYSTEM_PLATFORM = Get-Architecture -Info $SYSTEM_INFO -$SYSTEM_CPU_MAX = Set-ProcessorCount -Info $SYSTEM_INFO -$SYSTEM_OS_VERSION = Get-WindowsVersion -Info $SYSTEM_INFO -$VS_INSTANCE = Set-VsInstance -Automate $CI_CD_MODE -$VS_VERSION = Get-VsInstanceVersion -Config $VS_INSTANCE -$CMAKE_VERSION = Get-ProgramVersion -Program 'cmake' +$SYSTEM_PLATFORM = Get-Architecture -Info $SYSTEM_INFO +$SYSTEM_CPU_MAX = Set-ProcessorCount -Info $SYSTEM_INFO +$SYSTEM_OS_VERSION = Get-WindowsVersion -Info $SYSTEM_INFO +$VS_INSTANCE = Set-VsInstance -Automate $CI_CD_MODE +$VS_VERSION = Get-VsInstanceVersion -Config $VS_INSTANCE +$CMAKE_VERSION = Get-ProgramVersion -Program 'cmake' ### ### Functions @@ -196,37 +201,37 @@ Write-Output " Asset SUV version: $ASSET_SUV_VERSION" Write-Output '-----------------------------------------------------------------------------------------' Write-Output '' -# Ensure script is run from `AutonomySim` project directory +# Ensure script is run from `AutonomySim` project directory. Test-WorkingDirectory -# Test Visual Studio version (optionally automated for CI/CD) +# Test Visual Studio version (optionally automated for CI/CD). Test-VisualStudioVersion -Automate $CI_CD_MODE -# Test CMake version (downloads and installs CMake) +# Test CMake version (downloads and installs CMake). Test-CmakeVersion -# Create temporary directories if they do not exist +# Create temporary directories if they do not exist. Add-Directories -Directories @('temp', 'external', 'external\rpclib') -# Test RpcLib version (downloads and builds rpclib) -Test-RpcLibVersion +# Test Eigen library version. +Test-EigenVersion -# Test high-polycount SUV asset -Test-AssetSuvVersion -HighPolycountSuv $ASSET_SUV +# Test RpcLib version (downloads and builds rpclib). +Test-RpcLibVersion -CmakeGenerator "$CmakeGenerator" -# Test Eigen library version -Test-EigenVersion +# Test high-polycount SUV asset. +Test-AssetSuvVersion -HighPolycountSuv $ASSET_SUV -# Compile AutonomySim.sln including MavLinkCom +# Compile AutonomySim.sln including MavLinkCom. Build-Solution -BuildMode "$BUILD_MODE" -SystemPlatform "$SYSTEM_PLATFORM" -SystemCpuMax "$SYSTEM_CPU_MAX" -# Copy binaries and includes for MavLinkCom and Unreal/Plugins +# Copy binaries and includes for MavLinkCom and Unreal/Plugins. Copy-GeneratedBinaries -# Update all Unreal Engine environments under AutonomySim\Unreal\Environments +# Update all Unreal Engine environments under AutonomySim\Unreal\Environments. Update-VsUnrealProjectFiles -ProjectDir "$PROJECT_DIR" -# Optionally build documentation +# Build documentation (optional). if ($BUILD_DOCS) { Build-Documentation } exit 0 diff --git a/scripts/format_cpp.sh b/scripts/format_cpp.sh index 85a57db0..29ff595f 100644 --- a/scripts/format_cpp.sh +++ b/scripts/format_cpp.sh @@ -1,5 +1,4 @@ #!/bin/bash -# #---------------------------------------------------------------------------------------- # Filename # format_code.sh diff --git a/scripts/powershell_formatter.psm1 b/scripts/powershell_formatter.psm1 new file mode 100644 index 00000000..1cf3a99e --- /dev/null +++ b/scripts/powershell_formatter.psm1 @@ -0,0 +1,67 @@ +<# +FILENAME: + powershell_formatter.psm1 +DESCRIPTION: + PowerShell module for formatting scripts or directories. +AUTHOR: + Adam Erickson (Nervosys) +DATE: + 2024-02-20 +NOTES: + Assumes: PowerShell version >= 7. + + Copyright © 2024 Nervosys, LLC +#> + +# Install PowerShell module if not already installed. +Install-ModuleIfMissing { + [OutputType()] + param( + [Parameter(Mandatory, HelpMessage = 'Name of the PowerShell module.')] + [String] + $ModuleName, + [Parameter(HelpMessage = 'Switch to enable verbose mode.')] + [Switch] + $Silent + ) + if ( -not $Silent.IsPresent ) { Write-Output 'Checking if moduled is installed...' } + if ( -not (Get-Module -Name "$ModuleName" -ListAvailable | Select Name, Version) ) { + if ( -not $Silent.IsPresent ) { + Write-Output "Module not found: ${ModuleName}" + Write-Output "Installing module: ${ModuleName}" + } + Install-Module -Name "$ModuleName" -Force -Confirm:$false + } + else { + if ( -not $Silent.IsPresent ) { Write-Output "Module found: ${ModuleName}" } + } +} + +# Create array of PowerShell file (.ps1 .psm1) absolute paths. +Format-PowerShell { + [OutputType()] + param( + [Parameter(Mandatory, HelpMessage = 'Path to PowerShell script or directory for formatting.')] + [String] + $Path, + [Parameter(HelpMessage = 'Text file encoding.')] + [String] + $Encoding = 'utf8', + [Parameter(HelpMessage = 'Switch for recursively formatting files in directory.')] + [Switch] + $Recurse + ) + Install-ModuleIfMissing -ModuleName "PSScriptAnalyzer" + # Get a list of file paths. + [String[]]$FilePaths = if ( $Recurse.IsPresent ) { + (Get-ChildItem -Path $Path -File -Include ('*.ps1', '*.psm1') -Recurse | Select FullName) + } + else { $Path } + # Iterate over files, load to string, format string, save to file. + foreach ( $f in $FilePaths ) { + [String]$FileContents = (Get-Content -Encoding "$Encoding" -Path "$f" | Out-String) + Invoke-Formatter -ScriptDefinition $FileContents + Set-Content -Path "$f" -Encoding "$Encoding" -Value "$FileContents" + } + Write-Output 'PowerShell script formatting complete.' +} diff --git a/scripts/step_eigen.psm1 b/scripts/step_eigen.psm1 index 51b7429d..f71c041a 100644 --- a/scripts/step_eigen.psm1 +++ b/scripts/step_eigen.psm1 @@ -19,9 +19,9 @@ NOTES: # Common utilities Import-Module "${PWD}\scripts\utils.psm1" # imports: Add-Directories, Remove-Directories, Invoke-Fail, Test-WorkingDirectory, - # Test-VariableDefined, Get-EnvVariables, Get-ProgramVersion, Get-VersionMajorMinor, - # Get-VersionMajorMinorBuild, Get-WindowsInfo, Get-WindowsVersion, Get-Architecture, - # Get-ArchitectureWidth, Set-ProcessorCount +# Test-VariableDefined, Get-EnvVariables, Get-ProgramVersion, Get-VersionMajorMinor, +# Get-VersionMajorMinorBuild, Get-WindowsInfo, Get-WindowsVersion, Get-Architecture, +# Get-ArchitectureWidth, Set-ProcessorCount ### ### Variables @@ -35,20 +35,28 @@ Import-Module "${PWD}\scripts\utils.psm1" # imports: Add-Directori ### Functions ### -function Test-EigenVersion { - if ( -not (Test-Path -LiteralPath "$EIGEN_DIR") ) { - [System.IO.Directory]::CreateDirectory('AutonomyLib\deps') +function Get-Eigen { + [OutputType()] - [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12 - Invoke-WebRequest "$EIGEN_URL" -OutFile 'temp\eigen3.zip' - - Expand-Archive -Path 'temp\eigen3.zip' -DestinationPath 'AutonomyLib\deps' - Move-Item -Path AutonomyLib\deps\eigen* -Destination 'AutonomyLib\deps\del_eigen' + # Download Eigen. + [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12 + Invoke-WebRequest "$EIGEN_URL" -OutFile 'temp\eigen3.zip' - [System.IO.Directory]::CreateDirectory("$EIGEN_DIR") - Move-Item -Path 'AutonomyLib\deps\del_eigen\Eigen' -Destination "$EIGEN_DIR\Eigen" - Remove-Item 'AutonomyLib\deps\del_eigen' -Force -Recurse - Remove-Item 'temp\eigen3.zip' -Force + # Unpack Eigen. + [System.IO.Directory]::CreateDirectory('AutonomyLib\deps') + Expand-Archive -Path 'temp\eigen3.zip' -DestinationPath 'AutonomyLib\deps' + Move-Item -Path AutonomyLib\deps\eigen* -Destination 'AutonomyLib\deps\del_eigen' + + # Move Eigen directory into dependencies. + [System.IO.Directory]::CreateDirectory("$EIGEN_DIR") + Move-Item -Path 'AutonomyLib\deps\del_eigen\Eigen' -Destination "$EIGEN_DIR\Eigen" + Remove-Item 'AutonomyLib\deps\del_eigen' -Force -Recurse + Remove-Item 'temp\eigen3.zip' -Force +} + +function Test-EigenVersion { + if ( -not (Test-Path -LiteralPath "$EIGEN_DIR") ) { + Get-Eigen } if ( -not (Test-Path -LiteralPath "$EIGEN_DIR") ) { Write-Error "Eigen library directory not found: ${EIGEN_DIR}" -ErrorAction SilentlyContinue diff --git a/scripts/step_rpclib.psm1 b/scripts/step_rpclib.psm1 index 0aa40ab0..1573796a 100644 --- a/scripts/step_rpclib.psm1 +++ b/scripts/step_rpclib.psm1 @@ -63,6 +63,11 @@ function Get-RpcLib { } function Build-RpcLib { + param( + [Parameter(HelpMessage = 'Options: [ "Visual Studio 17 2022" | "Visual Studio 16 2019" ]')] + [String] + $CmakeGenerator = "$VS_GENERATOR" + ) Write-Output '' Write-Output '-----------------------------------------------------------------------------------------' Write-Output " Building rpclib version ${RPCLIB_VERSION} with CMake version ${CMAKE_VERSION}..." @@ -73,7 +78,9 @@ function Build-RpcLib { Set-Location "${RPCLIB_PATH}\build" Write-Output "Current directory: ${RPCLIB_PATH}\build" - Start-Process -FilePath 'cmake.exe' -ArgumentList "-G ${VS_GENERATOR}", '..' -Wait -NoNewWindow + # Generate RpcLib build files + Start-Process -FilePath 'cmake.exe' -ArgumentList "-G ${CmakeGenerator}", '..' -Wait -NoNewWindow + # Build RpcLib if ( $BUILD_MODE -eq 'Release' ) { Start-Process -FilePath 'cmake.exe' -ArgumentList '--build', '.' -Wait -NoNewWindow Start-Process -FilePath 'cmake.exe' -ArgumentList '--build', '.', '--config Release' -Wait -NoNewWindow @@ -104,12 +111,17 @@ function Build-RpcLib { } function Test-RpcLibVersion { + param( + [Parameter(HelpMessage = 'Options: [ "Visual Studio 17 2022" | "Visual Studio 16 2019" ]')] + [String] + $CmakeGenerator = "$VS_GENERATOR" + ) if ( -not (Test-Path -LiteralPath "$RPCLIB_PATH") ) { # Remove previous installations Remove-Item 'external\rpclib' -Force -Recurse # Download and build rpclib Get-RpcLib - Build-RpcLib + Build-RpcLib -CmakeGenerator "$CmakeGenerator" # Fail if rpclib version path not found if ( -not (Test-Path -LiteralPath "$RPCLIB_PATH") ) { Write-Error 'Error: Download and build of rpclib failed. Stopping build.' -ErrorAction SilentlyContinue diff --git a/scripts/step_visualstudio.psm1 b/scripts/step_visualstudio.psm1 index fb773a56..962831f9 100644 --- a/scripts/step_visualstudio.psm1 +++ b/scripts/step_visualstudio.psm1 @@ -8,7 +8,7 @@ AUTHOR: DATE: 2024-02-20 NOTES: - Assumes: PowerShell version >= 7 and Visual Studio 2022 (version 17). + Assumes: PowerShell version >= 7 and Visual Studio >= 2019 (version 16). Copyright © 2024 Nervosys, LLC #>