diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 815c00e5b4..a1c265f3fb 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -11,12 +11,16 @@ concurrency: cancel-in-progress: true jobs: - ubuntu22-tests: - name: Ubuntu 22.04 Build - runs-on: ubuntu-22.04 + multiplatform-build: + strategy: + matrix: + platform: [ ubuntu-22.04, ubuntu-24.04 ] + + name: Ubuntu Alternate Builds + runs-on: ${{ matrix.platform }} steps: # checks-out the repository under $GITHUB_WORKSPACE - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Environment Setup run: | @@ -25,24 +29,31 @@ jobs: - name: Software Build Test run: | cd src - bazel build --show_timestamps \ - -- //... -//software:unix_full_system \ - -//software/simulated_tests/... \ - -//software/ai/hl/... \ - -//software/field_tests/... \ - -//software/jetson_nano/... \ + bazel build --show_timestamps --copt=-O3 \ + -- //... -//software:unix_full_system \ + -//software/simulated_tests/... \ + -//software/ai/hl/... \ + -//software/field_tests/... \ + -//software/embedded/... \ + -//cc_toolchain/... - name: Jetson Nano Build Test run: | cd src - bazel build --cpu=jetson_nano //software/jetson_nano:thunderloop_main --copt=-O3 + bazel build //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=NANO --platforms=//cc_toolchain:robot + + - name: Raspberry Pi Build Test + run: | + cd src + bazel build //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=PI --platforms=//cc_toolchain:robot + software-tests: name: Software Tests runs-on: ubuntu-20.04 steps: # checks-out the repository under $GITHUB_WORKSPACE - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Environment Setup run: | @@ -56,14 +67,15 @@ jobs: -//software/simulated_tests/... \ -//software/ai/hl/... \ -//software/field_tests/... \ - -//software/ai/navigator/... + -//software/ai/navigator/... \ + -//cc_toolchain/... - jetson-nano-tests: - name: Jetson Nano Software Tests + robot-tests: + name: Robot Software Tests runs-on: ubuntu-20.04 steps: # checks-out the repository under $GITHUB_WORKSPACE - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Environment Setup run: | @@ -85,14 +97,21 @@ jobs: - name: Jetson Nano Build run: | cd src - bazel build --cpu=jetson_nano //software/jetson_nano:thunderloop_main --copt=-O3 + bazel build //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=NANO --platforms=//cc_toolchain:robot + + + - name: Raspberry Pi Build + run: | + cd src + bazel build //software/embedded:thunderloop_main --copt=-O3 --//software/embedded:host_platform=PI --platforms=//cc_toolchain:robot + simulated-gameplay-tests: name: Simulated Gameplay Tests runs-on: ubuntu-20.04 steps: # checks-out the repository under $GITHUB_WORKSPACE - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Environment Setup run: | @@ -102,35 +121,35 @@ jobs: run: | cd src bazel test --copt=-O3 --flaky_test_attempts=3 --show_timestamps \ - //software:unix_full_system \ - //software/simulated_tests/... \ - //software/ai/hl/... \ + //software:unix_full_system \ + //software/simulated_tests/... \ + //software/ai/hl/... \ //software/ai/navigator/... - name: Upload simulated test proto logs # Ensure that simulated test logs get uploaded if: always() - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: - name: blue-ai-proto-logs + name: blue-sim-test-proto-logs path: | - /tmp/tbots/blue/logs + /tmp/tbots/blue/test/*/proto_* - name: Upload simulated test proto logs # Ensure that simulated test logs get uploaded if: always() - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: - name: yellow-ai-proto-logs + name: yellow-sim-test-proto-logs path: | - /tmp/tbots/yellow/logs + /tmp/tbots/yellow/test/*/proto_* autorefd-game: name: AutoRef'd Game (3 Minutes) runs-on: ubuntu-20.04 steps: # checks-out the repository under $GITHUB_WORKSPACE - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Environment Setup run: | @@ -144,7 +163,7 @@ jobs: - name: Upload AI vs AI logs # Ensure that simulated test logs get uploaded if: always() - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: blue-ai-vs-ai-proto-logs path: | @@ -153,7 +172,7 @@ jobs: - name: Upload AI vs AI logs # Ensure that simulated test logs get uploaded if: always() - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: yellow-ai-vs-ai-proto-logs path: | diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index fc14faa4db..bcffc1dc83 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -18,7 +18,7 @@ jobs: if: github.event.pull_request.draft == false steps: # checks-out the repository under $GITHUB_WORKSPACE - - uses: actions/checkout@v2 + - uses: actions/checkout@v4 - name: Environment Setup run: | diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md index be8fd153c5..7dbe3e5538 100644 --- a/docs/fsm-diagrams.md +++ b/docs/fsm-diagrams.md @@ -314,6 +314,21 @@ Terminate:::terminate --> Terminate:::terminate ``` +## [HaltFSM](/src/software/ai/hl/stp/tactic/halt/halt_fsm.h) + +```mermaid + +stateDiagram-v2 +classDef terminate fill:white,color:black,font-weight:bold +direction LR +[*] --> StopState +StopState --> StopState : [!stopDone]\nupdateStop +StopState --> Terminate:::terminate : [stopDone]\nupdateStop +Terminate:::terminate --> StopState : [!stopDone]\nupdateStop +Terminate:::terminate --> Terminate:::terminate : [stopDone]\nupdateStop + +``` + ## [KickFSM](/src/software/ai/hl/stp/tactic/kick/kick_fsm.h) ```mermaid @@ -441,18 +456,3 @@ Terminate:::terminate --> Terminate:::terminate : SET_STOP_PRIMITIVE_ACTION StopState -StopState --> StopState : [!stopDone]\nupdateStop -StopState --> Terminate:::terminate : [stopDone]\nupdateStop -Terminate:::terminate --> StopState : [!stopDone]\nupdateStop -Terminate:::terminate --> Terminate:::terminate : [stopDone]\nupdateStop - -``` - diff --git a/docs/getting-started-wsl.md b/docs/getting-started-wsl.md index 999f86e7b8..1e48c769b8 100644 --- a/docs/getting-started-wsl.md +++ b/docs/getting-started-wsl.md @@ -39,7 +39,7 @@ If you are not using Windows 11 and would prefer not to upgrade, you can follow 3. Now, let's install Ubuntu. - Download the WSL2 kernel from [here](https://docs.microsoft.com/en-us/windows/wsl/wsl2-kernel). - Open a PowerShell window and run command `wsl --set-default-version 2` to use WSL2 by default. - - Install Ubuntu 20.04 LTS or Ubuntu 22.04 LTS from the Microsoft Store. + - Install Ubuntu 20.04 LTS, Ubuntu 22.04 LTS or Ubuntu 24.04 LTS from the Microsoft Store. - Open the Ubuntu app in the Start menu. It will open a command prompt and ask you to create a new UNIX username and password for your WSL2 Ubuntu installation. ### X Server Setup diff --git a/docs/getting-started.md b/docs/getting-started.md index c9aea63a5a..4c6d3bbd54 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -1,10 +1,5 @@ -Table of Contents -================= - +Table of Contents +================= * [Software Setup](#software-setup) * [Introduction](#introduction) @@ -30,8 +25,8 @@ Table of Contents * [Profiling](#profiling) * [Callgrind](#callgrind) * [Tracy](#tracy) - * [Building for Jetson Nano](#building-for-jetson-nano) - * [Deploying to Jetson Nano](#deploying-to-jetson-nano) + * [Building for the robot](#building-for-the-robot) + * [Deploying Robot Software to the robot](#deploying-robot-software-to-the-robot) * [Setting up Virtual Robocup 2021](#setting-up-virtual-robocup-2021) * [Setting up the SSL Simulation Environment](#setting-up-the-ssl-simulation-environment) * [Pushing a Dockerfile to dockerhub](#pushing-a-dockerfile-to-dockerhub) @@ -49,6 +44,14 @@ Table of Contents * [Example Workflow](#example-workflow) * [Testing](#testing) + + # Software Setup ## Introduction @@ -63,9 +66,15 @@ These instructions assume you have a basic understanding of Linux and the comman ### Operating systems -We currently only support Linux, specifically Ubuntu 20.04 LTS and Ubuntu 22.04 LTS. You are welcome to use a different version or distribution of Linux, but may need to make some tweaks in order for things to work. +We currently only support Linux, specifically Ubuntu. + +If you have a X86_64 machine, we support Ubuntu 20.04 LTS, Ubuntu 22.04 LTS and Ubuntu 24.04 LTS. -You can use Ubuntu 20.04 LTS and Ubuntu 22.04 LTS inside Windows through Windows Subsystem for Linux, by following [this guide](./getting-started-wsl.md). **Running and developing Thunderbots on Windows is experimental and not officially supported.** +If you have a ARM64 (also known as AARCH64) machine, we support Ubuntu 24.04 LTS. + +You are welcome to use a different version or distribution of Linux, but may need to make some tweaks in order for things to work. + +You can use Ubuntu 20.04 LTS, Ubuntu 22.04 LTS or Ubuntu 24.04 LTS inside Windows through Windows Subsystem for Linux, by following [this guide](./getting-started-wsl.md). **Running and developing Thunderbots on Windows is experimental and not officially supported.** ### Getting the Code @@ -339,17 +348,19 @@ Tracy also samples call stacks. If the profiled binary is run with root permissi ./tbots.py run thunderscope_main --tracy --sudo -## Building for Jetson Nano +## Building for the robot -To build for the Jetson Nano, build the target with the `--cpu=jetson_nano` flag and the toolchain will automatically build using the ARM toolchain for Jetson Nano. For example, `bazel build --cpu=jetson_nano //software/geom/...`. +To build for the robot computer, build the target with the `--platforms=//cc_toolchain:robot` flag and the toolchain will automatically build using the ARM toolchain. For example, `bazel build --platforms=//cc_toolchain:robot //software/geom/...`. -## Deploying to Jetson Nano +## Deploying Robot Software to the robot -We use ansible to automatically update software running on the Jetson Nano. [More info here.](useful-robot-commands.md#flashing-the-nano) +We use Ansible to automatically update software running on the robot. [More info here.](useful-robot-commands.md#flashing-the-robots-compute-module) To update binaries on a working robot, you can run: -`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook deploy_nano.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` + +Where `` is the robot platform you are deploying to (`PI` or `NANO`), and `` is the IP address of the robot you are deploying to. The `robot_password` is the password used to login to the `robot` user on the robot. ## Setting up Virtual Robocup 2021 diff --git a/docs/robot-software-architecture.md b/docs/robot-software-architecture.md index 8bd11d84d7..9f6059524b 100644 --- a/docs/robot-software-architecture.md +++ b/docs/robot-software-architecture.md @@ -16,11 +16,14 @@ ## Ansible -[Ansible](https://www.ansible.com/overview/how-ansible-works) allows us to run actions on multiple robots at once. Actions are communicated through YAML files called playbooks. Playbooks contain a series of tasks (ex move a file, run this script, output this command) and logic dictating dependencies between tasks. When playbooks are run, Ansible establishes an SSH connection between the user's computer and target Jetson Nanos, allowing it to run the tasks in the playbook. Output from each task, and any other requested output, is displayed on the console +[Ansible](https://www.ansible.com/overview/how-ansible-works) allows us to run actions on multiple robots at once. Actions are communicated through YAML files called playbooks. Playbooks contain a series of tasks (eg. move a file, run this script, output this command) and logic dictating dependencies between tasks. When playbooks are run, Ansible establishes an SSH connection between the user's computer and robot, allowing it to run the tasks in the playbook. Output from each task, and any other requested output, is displayed on the console For a more detailed look at how Ansible works, [see the RFC](https://docs.google.com/document/d/1hN3Us2Vjr8z6ihqUVp_3L7rrjKc-EZ-l2hZJc31gNOc/edit) -Example command: `bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook deploy_nano.yml --hosts --ssh_pass ` +Example command: `bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` +* : `PI` or `NANO` depending on the computer on the robot +* : IP address of the robot +* : Password of the robot More commands available [here](useful-robot-commands.md#off-robot-commands) @@ -36,14 +39,6 @@ To learn more about how it works, [see the RFC](https://docs.google.com/document # Thunderloop -Thunderloop is software that runs in a loop. It continuously polls services (unrelated from Systemd) sending relevant control proto (`PowerControl`, `MotorControl`) and receiving back status proto. Currently we have a [Network](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/jetson_nano/services/network/network.cpp), [Power](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/jetson_nano/services/power.cpp) and [Motor service](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/jetson_nano/services/motor.cpp). Thunderloop also receives `World` and `PrimitiveSet` Proto from AI and sends back Robot Status. +Thunderloop is software that runs in a loop. It continuously polls services (unrelated from Systemd), sending relevant control protos (`PowerControl`, `MotorControl`) and receiving back status protos from the power and motor boards. Currently we have a [Network](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/embedded/services/network/network.cpp), [Power](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/embedded/services/power.cpp) and [Motor](https://github.com/UBC-Thunderbots/Software/blob/master/src/software/embedded/services/motor.cpp) services. Thunderloop also receives `World` and `PrimitiveSet` protos from AI and sends back `Robot Status` protos. -Motor and Power service both interface with their respective electrical boards over different communication interfaces, namely SPI and UART respectively. - -# Announcements - -Broadcasts packets for what robot IDs are connected to wifi and what IPs they have. Combined with `robot_broadcast_receiver.py` allows us to see all robots on the network. - -# Display - -Displays information about the robot for us. Uses redis to receive info from other processes. Can also change values with physical buttons. +Motor and Power services both interface with their respective electrical boards over different communication interfaces, namely SPI and UART respectively. diff --git a/docs/useful-robot-commands.md b/docs/useful-robot-commands.md index e5399fa8d6..431f7c3c45 100644 --- a/docs/useful-robot-commands.md +++ b/docs/useful-robot-commands.md @@ -1,32 +1,40 @@ -# Common Robot Commands +Table of Contents +================= -# Table of Contents +* [Table of Contents](#table-of-contents) * [Common Debugging Steps](#common-debugging-steps) * [Off Robot Commands](#off-robot-commands) * [Wifi Disclaimer](#wifi-disclaimer) - * [Miscellaneous Ansible Tasks & Options](#miscellaneous-ansible-tasks--options) - * [Flashing the nano](#flashing-the-nano) + * [Miscellaneous Ansible Tasks & Options](#miscellaneous-ansible-tasks--options) + * [Flashing the robot's compute module](#flashing-the-robots-compute-module) * [Flashing the powerboard](#flashing-the-powerboard) - * [Setting up nano](#setting-up-nano) + * [Setting up the embedded host](#setting-up-the-embedded-host) + * [Jetson Nano](#jetson-nano) + * [Raspberry Pi](#raspberry-pi) * [Robot Diagnostics](#robot-diagnostics) + * [For Just Diagnostics](#for-just-diagnostics) + * [For AI + Diagnostics](#for-ai--diagnostics) * [Robot Auto Test](#robot-auto-test) * [On Robot Commands](#on-robot-commands) * [Systemd Services](#systemd-services) * [Debugging Uart](#debugging-uart) * [Redis](#redis) + + + # Common Debugging Steps ```mermaid --- title: Robot Debugging Steps --- flowchart TD - ssh(Can you SSH into the robot? - `ssh robot@192.168.0.20RobotID` OR `ssh robot@robot_name.local` - E.g. `ssh robot@192.168.0.203` or `ssh robot@robert.local` - for a robot called robert with robot id 3) + ssh("Can you SSH into the robot? + `ssh robot@192.168.0.20RobotID` (for Nanos) OR `ssh robot@192.168.5.20RobotID` (for Pis) OR `ssh robot@robot_name.local` + e.g. `ssh robot@192.168.0.203` (for Nanos) or `ssh robot@192.168.5.203` (for Pis) or `ssh robot@robert.local` + for a robot called robert with robot id 3") ssh ---> |Yes| tloop_status - ssh --> |No - Second Try| monitor("`Connect Jetson to an external monitor and check wifi connection _or_ SSH using an ethernet cable`") + ssh --> |No - Second Try| monitor("Connect Jetson or Pi to an external monitor and check wifi connection or SSH using an ethernet cable") ssh --> |No - First Try| restart(Restart robot) restart --> ssh @@ -53,12 +61,12 @@ flowchart TD `service thunderloop restart`) tloop_status --> |Running| tloop_logs(Check Thunderloop logs for errors `journalctl -fu thunderloop -n 300`) - tloop_logs --> |No Errors| check_redis(Does `redis-cli get /network_interface` return 'wlan0', + tloop_logs --> |No Errors| check_redis(Does `redis-cli get /network_interface` return 'wlan0' or 'tbots', and does `redis-cli get /channel_id` return '0'?) - tloop_logs --> |Contains Errors| rip2("`Fix errors or check errors with a lead`") - check_redis --> |No| update_redis(Update Redis constants by running: - `redis-cli set /network_interface 'wlan0'` - `redis-cli set /channel_id '0'`) + tloop_logs --> |Contains Errors| rip2("Fix errors or check errors with a lead") + check_redis --> |No| update_redis("Update Redis constants by running: + `redis-cli set /network_interface 'wlan0'` (for Nanos) OR `redis-cli set /network_interface 'tbots'` (for Pis) + `redis-cli set /channel_id '0'`") check_redis --> |Yes| rip3(Check with a lead) update_redis --> tloop_restart tloop_restart --> tloop_status @@ -78,14 +86,14 @@ The IP address of the robots on the tbots network is `192.168.0.20` so Individual miscellaneous tasks (ex reboot, shutdown, rtt test) can be run through the `misc.yml` playbook by specifying the corresponding tag. -To view a list of supported arguments, run -`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- -h` +To view a list of supported arguments, run: +`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot -h` If desired, the `-ho`, `--hosts` argument can be replaced with `-p`, `--port`, defining a port to listen to for Announcements from hosts. The `--tags` argument can be used to specify which actions to perform and on which services. -## Flashing the nano +## Flashing the robot's compute module This will stop the current Systemd services, replace and restart them. Binaries that are used for Systemd services are located in a folder in `home/robot` (the default directory) called `thunderbots_binaries`. @@ -93,31 +101,43 @@ This will stop the current Systemd services, replace and restart them. Binaries This will trigger motor calibration meaning the wheels may spin. Please elevate the robot so the wheels are not touching the ground for proper calibration. -`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook deploy_nano.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform= -- --playbook deploy_robot_software.yml --hosts --ssh_pass ` +* \ is the host platform on the robot (either `PI` or `NANO`) +* is the IP address of the robot +* is the password of the `robot` user account -You could also use the `tbots.py` script to flash +You could also use the `tbots.py` script to flash robot software -`./tbots.py run run_ansible -f -pwd ` (Note that this uses robot IDs rather than full robot IP addresses) +`./tbots.py run run_ansible -pl -f -pwd ` (Note that this uses robot IDs rather than full robot IP addresses) +* \ is the host platform on the robot (either `PI` or `NANO` +* is a list of robot IDs to flash +* is the password of the `robot` user account -Example: Flashing robots 1, 4, and 7 +Example: Flashing robots 1, 4, and 7 that have a Raspberry Pi -`./tbots.py run run_ansible -f 1 4 7 -pwd ` +`./tbots.py run run_ansible -pl PI -f 1 4 7 -pwd ` ## Flashing the powerboard -This will flash powerloop, the current firmware in `software/power/`, onto the power board. It will prompt the user into setting the powerboard into bootloader mode by holding the boot button (left if looking from the back of the robot) and pressing the reset button (right if looking from the back of the robot), then releasing the reset button first, then the boot button. Once the board is flashed, pressing the reset button after to use the new firmware. +This will flash powerloop, the current firmware in `software/power/`, onto the power board. It will prompt the user into setting the powerboard into bootloader mode by holding the boot button (left if looking from the back of the robot) and pressing the reset button (right if looking from the back of the robot), then releasing the reset button first, then the boot button. Once the board is flashed, pressing the reset button after to use the new firmware. Looking from the back of the robot the reset and boot buttons are on right side of the battery holder on the lowest board with the reset being on the left and the boot on the right. Warning it may kick/chip when pressed. -`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook deploy_powerboard.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot -- --playbook deploy_powerboard.yml --hosts --ssh_pass ` + +## Setting up the embedded host + +This section refers to setting up the computer on the robot for the first time. We need to setup dependencies, drivers and necessary configurations. + +Setting up the robot for the first time requires internet access -## Setting up nano +### Jetson Nano -This refers to setting up the Jetson Nano for the first time. This will enable Systemd services, modify device tree files and perform other setup necessary for the communication protocols used. +`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform=NANO -- --playbook setup_nano.yml --hosts --ssh_pass ` -Setting up the nano for the first time requires internet access +### Raspberry Pi -`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook setup_nano.yml --hosts --ssh_pass ` +`bazel run //software/embedded/ansible:run_ansible --platforms=//cc_toolchain:robot --//software/embedded:host_platform=PI -- --playbook setup_raspberry_pi.yml --hosts --ssh_pass ` ## Robot Diagnostics @@ -125,13 +145,13 @@ Robot Diagnostics allow users to input various commands to the robots. It can be If multiple people are using robot diagnostics at the same time on the same network please make sure each person only connects to the robots they are testing via the checkboxes -###For Just Diagnostics +### For Just Diagnostics From Software/src `./tbots.py run thunderscope --run_diagnostics --interface ` -###For AI + Diagnostics +### For AI + Diagnostics From Software/src @@ -144,9 +164,10 @@ Runs the robot auto test fixture on a robot through Ansible, which tests the mot From Software/src: -`bazel run //software/jetson_nano/ansible:run_ansible --cpu=jetson_nano -- --playbook robot_auto_test_playbook.yml --hosts --ssh_pass ` -* replace the with the actual ip address of the jetson nano for the ssh connection. -* replace the with the actual password for the jetson nano for the ssh connection. +`bazel run //software/embedded/ansible:run_ansible --//software/embedded:host_platform= --platforms=//cc_toolchain:robot -- --playbook robot_auto_test_playbook.yml --hosts --ssh_pass ` +* replace the \ with the target platform for the robot (either `PI` or `NANO`) +* replace the \ with the actual ip address of the jetson nano for the ssh connection. +* replace the with the actual password for the jetson nano for the ssh connection. # On Robot Commands diff --git a/environment_setup/setup_software.sh b/environment_setup/setup_software.sh index d9531f5173..422439bb77 100755 --- a/environment_setup/setup_software.sh +++ b/environment_setup/setup_software.sh @@ -11,12 +11,6 @@ # unit tests #~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~# -print_status_msg () { - echo "================================================================" - echo $1 - echo "================================================================" -} - # Save the parent dir of this so we can always run commands relative to the # location of this script, no matter where it is called from. This # helps prevent bugs and odd behaviour if this script is run through a symlink @@ -24,6 +18,11 @@ print_status_msg () { CURR_DIR=$(dirname -- "$(readlink -f -- "$BASH_SOURCE")") cd "$CURR_DIR" || exit +source util.sh + +arch=$(uname -m) +print_status_msg "Detected architecture: ${arch}" + print_status_msg "Installing Utilities and Dependencies" sudo apt-get update @@ -49,10 +48,10 @@ host_software_packages=( codespell # Fixes typos curl default-jdk # Needed for Bazel to run properly - gcc-9 # We use gcc 9.3.0 + gcc-10 # Full system compiles with gcc 10 libstdc++6-9-dbg git # required for build - g++-9 + g++-10 kcachegrind # This lets us view the profiles output by callgrind libeigen3-dev # A math / numerical library used for things like linear regression libprotobuf-dev @@ -62,9 +61,9 @@ host_software_packages=( protobuf-compiler # This is required for the "NanoPb" library, which does not # properly manage this as a bazel dependency, so we have # to manually install it ourselves - python3.10 # Python 3 - python3.10-dev # Python 3 headers - python3.10-venv # Virtual Environment + python3.12 # Python 3 + python3.12-dev # Python 3 headers + python3.12-venv # Virtual Environment python3-pip # Required for bazel to install python dependencies for build targets python3-protobuf # This is required for the "NanoPb" library, which does not # properly manage this as a bazel dependency, so we have @@ -90,17 +89,27 @@ if [[ $(lsb_release -rs) == "20.04" ]]; then host_software_packages+=(llvm-6.0) host_software_packages+=(libclang-6.0-dev) host_software_packages+=(libncurses5) - host_software_packages+=(qt5-default) # This fixes missing headers by notifying the linker - ldconfig + sudo ldconfig fi -if [[ $(lsb_release -rs) == "22.04" ]]; then - host_software_packages+=(qtbase5-dev) +# Clear the download cache +rm -rf /tmp/tbots_download_cache +mkdir /tmp/tbots_download_cache + +if [[ $(lsb_release -rs) == "22.04" ]] || [[ $(lsb_release -rs) == "24.04" ]]; then + # This is required because a Braille TTY device that Linux provides a driver for conflicts with the ESP32 + wget -nc https://github.com/UBC-Thunderbots/Software-External-Dependencies/blob/main/85-brltty.rules -O /tmp/tbots_download_cache/85-brltty.rules + sudo mv /tmp/tbots_download_cache/85-brltty.rules /usr/lib/udev/rules.d/85-brltty.rules +fi - wget -nc https://github.com/UBC-Thunderbots/Software-External-Dependencies/blob/main/85-brltty.rules -O /tmp/85-brltty.rules - sudo mv /tmp/85-brltty.rules /usr/lib/udev/rules.d/85-brltty.rules +virtualenv_opt_args="" +if [[ $(lsb_release -rs) == "24.04" ]]; then + host_software_packages+=(python3-pyqt6) + host_software_packages+=(pyqt6-dev-tools) + + virtualenv_opt_args="--system-site-packages" fi if ! sudo apt-get install "${host_software_packages[@]}" -y ; then @@ -114,14 +123,14 @@ print_status_msg "Setting Up Virtual Python Environment" # delete tbotspython first sudo rm -rf /opt/tbotspython -if ! curl -sS https://bootstrap.pypa.io/get-pip.py | sudo /usr/bin/python3.10 ; then - print_status_msg "Error: Installing pip failed" +if ! sudo /usr/bin/python3.12 -m venv /opt/tbotspython $virtualenv_opt_args ; then + print_status_msg "Error: Setting up virtual environment failed" exit 1 fi -if ! sudo /usr/bin/python3.10 -m venv /opt/tbotspython ; then - print_status_msg "Error: Setting up virtual environment failed" - exit 1 +if [[ $(lsb_release -rs) == "20.04" ]] || [[ $(lsb_release -rs) == "22.04" ]]; then + # Install pip if it is not a system-managed package + sudo /usr/bin/python3.12 -m ensurepip fi if ! sudo /opt/tbotspython/bin/python3 -m pip install --upgrade pip ; then @@ -137,32 +146,23 @@ if [[ $(lsb_release -rs) == "22.04" ]]; then sudo /opt/tbotspython/bin/pip3 install -r ubuntu22_requirements.txt fi -print_status_msg "Done Setting Up Virtual Python Environment" -print_status_msg "Fetching game controller" +if [[ $(lsb_release -rs) == "24.04" ]]; then + sudo /opt/tbotspython/bin/pip3 install -r ubuntu24_requirements.txt +fi sudo chown -R $USER:$USER /opt/tbotspython -sudo wget -N https://github.com/RoboCup-SSL/ssl-game-controller/releases/download/v2.15.2/ssl-game-controller_v2.15.2_linux_amd64 -O /opt/tbotspython/gamecontroller -sudo chmod +x /opt/tbotspython/gamecontroller + +print_status_msg "Done Setting Up Virtual Python Environment" +print_status_msg "Fetching game controller" +install_gamecontroller $arch print_status_msg "Setting up TIGERS AutoRef" -print_status_msg "Installing TIGERS dependency: Java 17" -sudo wget -N https://download.oracle.com/java/17/archive/jdk-17.0.5_linux-x64_bin.deb -O /tmp/jdk-17.0.5.deb -sudo apt install /tmp/./jdk-17.0.5.deb +print_status_msg "Installing TIGERS dependency: Java 21" +install_java $arch print_status_msg "Compiling TIGERS AutoRef" -sudo wget -N https://github.com/TIGERs-Mannheim/AutoReferee/archive/refs/heads/autoref-ci.zip -O /tmp/autoref-ci.zip -unzip -q -o -d /tmp/ /tmp/autoref-ci.zip -touch /tmp/AutoReferee-autoref-ci/.git # a hacky way to make gradle happy when it tries to find a dependency - -if ! /tmp/AutoReferee-autoref-ci/./gradlew installDist -p /tmp/AutoReferee-autoref-ci/ -Dorg.gradle.java.home=/usr/lib/jvm/jdk-17/; then - print_status_msg "Building TIGERS AutoRef failed. Downloading mirror" - - wget https://github.com/UBC-Thunderbots/AutoReferee/releases/download/autoref-ci/autoReferee.tar.gz -O /tmp/autoReferee.tar.gz - tar -xzf /tmp/autoReferee.tar.gz -C /opt/tbotspython/ -else - cp -r /tmp/AutoReferee-autoref-ci/build/install/autoReferee/ /opt/tbotspython/autoReferee -fi +install_autoref $arch sudo chmod +x "$CURR_DIR/../src/software/autoref/run_autoref.sh" sudo cp "$CURR_DIR/../src/software/autoref/DIV_B.txt" "/opt/tbotspython/autoReferee/config/geometry/DIV_B.txt" @@ -172,16 +172,14 @@ print_status_msg "Finished setting up AutoRef" # Install Bazel print_status_msg "Installing Bazel" -# Uninstall Bazel first -sudo rm -rf $HOME/.bazel - -# Adapted from https://docs.bazel.build/versions/main/install-ubuntu.html#install-with-installer-ubuntu -sudo wget -nc https://github.com/bazelbuild/bazel/releases/download/5.4.0/bazel-5.4.0-installer-linux-x86_64.sh -O /tmp/bazel-installer.sh -sudo chmod +x /tmp/bazel-installer.sh -sudo /tmp/bazel-installer.sh --bin=/usr/bin --base=$HOME/.bazel -echo "source ${HOME}/.bazel/bin/bazel-complete.bash" >> ~/.bashrc +install_bazel $arch print_status_msg "Done Installing Bazel" + +print_status_msg "Install clang-format" +install_clang_format $arch +print_status_msg "Done installing clang-format" + print_status_msg "Setting Up PlatformIO" # setup platformio to compile arduino code @@ -200,11 +198,16 @@ sudo service udev restart sudo usermod -a -G dialout $USER # install PlatformIO to global environment -if ! sudo /usr/bin/python3.10 -m pip install platformio==6.1.13; then +wget -O /tmp/tbots_download_cache/get-platformio.py https://raw.githubusercontent.com/platformio/platformio-core-installer/master/get-platformio.py +if ! /usr/bin/python3.12 /tmp/tbots_download_cache/./get-platformio.py; then print_status_msg "Error: Installing PlatformIO failed" exit 1 fi +# link platformio to /usr/local/bin so that bazel can find it +sudo rm /usr/local/bin/platformio +sudo ln -s ~/.platformio/penv/bin/platformio /usr/local/bin/platformio + print_status_msg "Done PlatformIO Setup" print_status_msg "Done Software Setup, please reboot for changes to take place" diff --git a/environment_setup/ubuntu20_requirements.txt b/environment_setup/ubuntu20_requirements.txt index 05af883b46..e521b08ed0 100644 --- a/environment_setup/ubuntu20_requirements.txt +++ b/environment_setup/ubuntu20_requirements.txt @@ -1,12 +1,10 @@ -protobuf==3.20.2 pyqtgraph==0.13.7 -pyqtdarktheme==2.1.0 -pyqt6==6.5.0 -PyQt6-Qt6==6.5.0 +pyqt6==6.6.1 +PyQt6-Qt6==6.6.1 thefuzz==0.19.0 iterfzf==0.5.0.20.0 -python-Levenshtein==0.12.2 +python-Levenshtein==0.25.1 psutil==5.9.0 PyOpenGL==3.1.6 -numpy==1.24.4 +qt-material==2.12 ruff==0.5.5 diff --git a/environment_setup/ubuntu22_requirements.txt b/environment_setup/ubuntu22_requirements.txt index 05af883b46..3a9e4d6385 100644 --- a/environment_setup/ubuntu22_requirements.txt +++ b/environment_setup/ubuntu22_requirements.txt @@ -1,12 +1,11 @@ -protobuf==3.20.2 pyqtgraph==0.13.7 -pyqtdarktheme==2.1.0 -pyqt6==6.5.0 -PyQt6-Qt6==6.5.0 +pyqt6==6.6.1 +PyQt6-Qt6==6.6.1 thefuzz==0.19.0 iterfzf==0.5.0.20.0 -python-Levenshtein==0.12.2 +python-Levenshtein==0.25.1 psutil==5.9.0 PyOpenGL==3.1.6 -numpy==1.24.4 +numpy==1.26.4 +qt-material==2.12 ruff==0.5.5 diff --git a/environment_setup/ubuntu24_requirements.txt b/environment_setup/ubuntu24_requirements.txt new file mode 100644 index 0000000000..983fcdda89 --- /dev/null +++ b/environment_setup/ubuntu24_requirements.txt @@ -0,0 +1,8 @@ +pyqtgraph==0.13.7 +thefuzz==0.19.0 +iterfzf==0.5.0.20.0 +python-Levenshtein==0.25.1 +psutil==5.9.0 +PyOpenGL==3.1.6 +qt-material==2.12 +ruff==0.5.5 diff --git a/environment_setup/util.sh b/environment_setup/util.sh new file mode 100755 index 0000000000..e72046ad56 --- /dev/null +++ b/environment_setup/util.sh @@ -0,0 +1,91 @@ +install_autoref() { + autoref_commit=b30660b78728c3ce159de8ae096181a1ec52e9ba + sudo wget -N https://github.com/TIGERs-Mannheim/AutoReferee/archive/${autoref_commit}.zip -O /tmp/tbots_download_cache/autoReferee.zip + unzip -q -o -d /tmp/tbots_download_cache/ /tmp/tbots_download_cache/autoReferee.zip + + /tmp/tbots_download_cache/AutoReferee-${autoref_commit}/./gradlew installDist -p /tmp/tbots_download_cache/AutoReferee-${autoref_commit} -Dorg.gradle.java.home=/opt/tbotspython/bin/jdk + mv /tmp/tbots_download_cache/AutoReferee-${autoref_commit}/build/install/autoReferee /opt/tbotspython/ +} + +install_bazel() { + download=https://github.com/bazelbuild/bazel/releases/download/5.4.0/bazel-5.4.0-linux-arm64 + + if is_x86 $1; then + download=https://github.com/bazelbuild/bazel/releases/download/5.4.0/bazel-5.4.0-linux-x86_64 + fi + + wget -nc $download -O /tmp/tbots_download_cache/bazel + sudo mv /tmp/tbots_download_cache/bazel /usr/bin/bazel + sudo chmod +x /usr/bin/bazel +} + +install_clang_format() { + download=https://github.com/llvm/llvm-project/releases/download/llvmorg-10.0.0/clang+llvm-10.0.0-aarch64-linux-gnu.tar.xz + clang_format_path=/tmp/tbots_download_cache/clang+llvm-10.0.0-aarch64-linux-gnu/bin/clang-format + + if is_x86 $1; then + download=https://github.com/llvm/llvm-project/releases/download/llvmorg-10.0.0/clang+llvm-10.0.0-x86_64-linux-gnu-ubuntu-18.04.tar.xz + clang_format_path=/tmp/tbots_download_cache/clang+llvm-10.0.0-x86_64-linux-gnu-ubuntu-18.04/bin/clang-format + fi + + wget $download -O /tmp/tbots_download_cache/clang.tar.xz + tar -xf /tmp/tbots_download_cache/clang.tar.xz -C /tmp/tbots_download_cache/ + sudo cp $clang_format_path /opt/tbotspython/bin/clang-format +} + +install_gamecontroller () { + # TODO(#3335): Whenever we deprecate Ubuntu 20.04, we can just grab the latest version of the SSL game controller + # binary from the releases page. This is a workaround since the latest version of the game controller is compiled + # with a newer GLIBC version than what is available on Ubuntu 20.04. + go_arch=arm64 + if is_x86 $1; then + go_arch=amd64 + fi + sudo wget -N https://go.dev/dl/go1.23.0.linux-${go_arch}.tar.gz -O /tmp/tbots_download_cache/go.tar.gz + tar -C /tmp/tbots_download_cache -xf /tmp/tbots_download_cache/go.tar.gz + export PATH=$PATH:/tmp/tbots_download_cache/go/bin + sudo wget -N https://github.com/RoboCup-SSL/ssl-game-controller/archive/refs/tags/v3.12.3.zip -O /tmp/tbots_download_cache/ssl-game-controller.zip + unzip -q -o -d /tmp/tbots_download_cache/ /tmp/tbots_download_cache/ssl-game-controller.zip + cd /tmp/tbots_download_cache/ssl-game-controller-3.12.3 + + # Installing nvm + wget -qO- https://raw.githubusercontent.com/nvm-sh/nvm/v0.40.1/install.sh | bash + export NVM_DIR="$HOME/.nvm" + [ -s "$NVM_DIR/nvm.sh" ] && \. "$NVM_DIR/nvm.sh" # This loads nvm + [ -s "$NVM_DIR/bash_completion" ] && \. "$NVM_DIR/bash_completion" # This loads nvm bash_completion + nvm install 20 + + make install + go build cmd/ssl-game-controller/main.go + sudo mv main /opt/tbotspython/gamecontroller + sudo chmod +x /opt/tbotspython/gamecontroller + + cd - + sudo rm -rf /tmp/tbots_download_cache/ssl-game-controller-3.12.3 + sudo rm -rf /tmp/tbots_download_cache/go +} + +install_java () { + java_home="" + java_download=https://download.oracle.com/java/21/latest/jdk-21_linux-aarch64_bin.tar.gz + if is_x86 $1; then + java_download=https://download.oracle.com/java/21/latest/jdk-21_linux-x64_bin.tar.gz + fi + sudo wget -N $java_download -O /tmp/tbots_download_cache/jdk-21.tar.gz + tar -xzf /tmp/tbots_download_cache/jdk-21.tar.gz -C /opt/tbotspython/ + mv /opt/tbotspython/jdk-21* /opt/tbotspython/bin/jdk +} + +is_x86() { + if [[ $1 == "x86_64" ]]; then + return 0 + else + return 1 + fi +} + +print_status_msg () { + echo "================================================================" + echo $1 + echo "================================================================" +} diff --git a/scripts/clang-format-10.0 b/scripts/clang-format-10.0 deleted file mode 100755 index c3b9422c90..0000000000 Binary files a/scripts/clang-format-10.0 and /dev/null differ diff --git a/scripts/compile_pip_requirements.sh b/scripts/compile_pip_requirements.sh index dd2bce5e88..becf00a925 100755 --- a/scripts/compile_pip_requirements.sh +++ b/scripts/compile_pip_requirements.sh @@ -17,5 +17,5 @@ BAZEL_ROOT_DIR="$CURR_DIR/../src" cd $BAZEL_ROOT_DIR bazel run //extlibs:nanopb_requirements.update bazel run //software/thunderscope:requirements.update -bazel run //software/jetson_nano/ansible:requirements.update +bazel run //software/embedded/ansible:requirements.update bazel run //software/simulated_tests:requirements.update diff --git a/scripts/lint_and_format.sh b/scripts/lint_and_format.sh index 8982f4ff5a..0f6fc83187 100755 --- a/scripts/lint_and_format.sh +++ b/scripts/lint_and_format.sh @@ -10,6 +10,9 @@ export CLANG_VERSION=10.0 # The directory this script is in CURR_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" +# The path to clang-format +export CLANG_BIN=/opt/tbotspython/bin/clang-format + # The root bazel directory BAZEL_ROOT_DIR="$CURR_DIR/../src" @@ -33,7 +36,7 @@ function run_clang_format () { # clang-format as arguments # We remove the last -o flag from the extension string find $CURR_DIR/../src/ ${EXTENSION_STRING::-2} \ - | xargs -I{} -n1000 $CURR_DIR/clang-format-$CLANG_VERSION -i -style=file + | xargs -I{} -n1000 $CLANG_BIN -i -style=file if [[ "$?" != 0 ]]; then printf "\n***Failed to run clang-format over all files!***\n\n" diff --git a/src/.bazelrc b/src/.bazelrc index 1df5a1234b..8f694e5535 100644 --- a/src/.bazelrc +++ b/src/.bazelrc @@ -10,7 +10,7 @@ test --test_output=all build --crosstool_top=//cc_toolchain:toolchain # Add warnings to Thunderbots code only -build --per_file_copt=//proto/message_translation/.*,//proto/primitive/.*,//software/.*,//shared/.*@-Wall,-Wextra,-Wconversion,-Wno-unused-parameter,-Wno-deprecated,-Werror +build --per_file_copt=//proto/.*,//proto/message_translation/.*,//proto/primitive/.*,//software/.*,//shared/.*@-Wall,-Wextra,-Wconversion,-Wno-unused-parameter,-Wno-deprecated,-Werror,-Wno-deprecated-declarations # Warn variable length arrays only when compiling cpp build --per_file_copt=.*\.cpp@-Wvla @@ -31,5 +31,9 @@ build --test_env=XDG_RUNTIME_DIR build --test_env=DISPLAY # Setup python bin/lib to point to our venv -build --action_env=PYTHON_BIN_PATH=/opt/tbotspython/bin/python3.10 -build --action_env=PYTHON_LIB_PATH=/opt/tbotspython/lib/python3.10 +build --action_env=PYTHON_BIN_PATH=/opt/tbotspython/bin/python3.12 +build --action_env=PYTHON_LIB_PATH=/opt/tbotspython/lib/python3.12 + +# Enable the new cc_toolchain resolution strategy +# #TODO(#3382): Delete when we upgrade to Bazel 7.0 +build --incompatible_enable_cc_toolchain_resolution diff --git a/src/MODULE.bazel b/src/MODULE.bazel new file mode 100644 index 0000000000..bfd76eecbf --- /dev/null +++ b/src/MODULE.bazel @@ -0,0 +1 @@ +bazel_dep(name = "bazel_skylib", version = "1.5.0") diff --git a/src/WORKSPACE b/src/WORKSPACE index 790a388d2c..1814824150 100644 --- a/src/WORKSPACE +++ b/src/WORKSPACE @@ -46,7 +46,7 @@ pip_parse( pip_parse( name = "ansible_deps", python_interpreter = "/opt/tbotspython/bin/python", - requirements_lock = "//software/jetson_nano/ansible:requirements_lock.txt", + requirements_lock = "//software/embedded/ansible:requirements_lock.txt", ) pip_parse( @@ -104,9 +104,9 @@ http_archive( http_archive( name = "g3log", build_file = "@//extlibs:g3log.BUILD", - sha256 = "176fcf4e1634aba425549c32f76426d7976ab6973370785fd6b76986e9f7b20e", - strip_prefix = "g3log-1.3.3", - url = "https://github.com/KjellKod/g3log/archive/1.3.3.zip", + sha256 = "2177e6dfd86fa7465c44c8ef5c3b6ab98ffc94e1130355d6982f7d886cb7bec9", + strip_prefix = "g3log-2.4", + url = "https://github.com/KjellKod/g3log/archive/refs/tags/2.4.zip", ) http_archive( @@ -142,9 +142,9 @@ protobuf_deps() git_repository( name = "com_github_nelhage_rules_boost", - commit = "7332c6cf2afb2642f53bc23f9b4d9c1817318685", + commit = "58be4e7e851d19e9ba14ced7bdba6fc8895af1d3", remote = "https://github.com/nelhage/rules_boost", - shallow_since = "1606097530 -0500", + shallow_since = "1724946929 +0200", ) git_repository( @@ -214,25 +214,15 @@ git_repository( shallow_since = "1571237073 -0500", ) -# Qt bazel rules from https://github.com/justbuchanan/bazel_rules_qt -git_repository( - name = "bazel_rules_qt", - commit = "7665177f47bf514176d5f8575a7334f030203e3d", - remote = "https://github.com/justbuchanan/bazel_rules_qt.git", - shallow_since = "1614109311 -0800", -) - -# Right now qt5 is installed on the system using the setup_software.sh script -# We assume we can find the headers in the "standard" path used in this rule new_local_repository( - name = "qt", - build_file = "@bazel_rules_qt//:qt.BUILD", - path = "/usr/include/x86_64-linux-gnu/qt5/", + name = "linux_k8_gcc", + build_file = "@//extlibs:linux_k8_gcc.BUILD", + path = "/", ) new_local_repository( - name = "linux_gcc", - build_file = "@//extlibs:linux_gcc.BUILD", + name = "linux_aarch64_gcc", + build_file = "@//extlibs:linux_aarch64_gcc.BUILD", path = "/", ) @@ -244,8 +234,8 @@ new_local_repository( ) http_archive( - name = "jetson_nano_gcc", - build_file = "@//extlibs:jetson_nano_gcc.BUILD", + name = "k8_jetson_nano_cross_compile_gcc", + build_file = "@//extlibs:k8_jetson_nano_cross_compile_gcc.BUILD", sha256 = "73eed74e593e2267504efbcf3678918bb22409ab7afa3dc7c135d2c6790c2345", strip_prefix = "gcc-linaro-7.3.1-2018.05-x86_64_aarch64-linux-gnu", urls = [ @@ -348,9 +338,9 @@ http_archive( http_archive( name = "pybind11", build_file = "@pybind11_bazel//:pybind11.BUILD", - sha256 = "1c6e0141f7092867c5bf388bc3acdb2689ed49f59c3977651394c6c87ae88232", - strip_prefix = "pybind11-2.9.0", - urls = ["https://github.com/pybind/pybind11/archive/refs/tags/v2.9.0.zip"], + sha256 = "411f77380c43798506b39ec594fc7f2b532a13c4db674fcf2b1ca344efaefb68", + strip_prefix = "pybind11-2.12.0", + urls = ["https://github.com/pybind/pybind11/archive/refs/tags/v2.12.0.zip"], ) load("@pybind11_bazel//:python_configure.bzl", "python_configure") @@ -402,3 +392,9 @@ new_git_repository( remote = "https://github.com/wolfpld/tracy.git", shallow_since = "1697482622 +0200", ) + +register_toolchains( + "//cc_toolchain:cc_toolchain_for_k8_jetson_nano_cross_compile", + "//cc_toolchain:cc_toolchain_for_k8", + "//cc_toolchain:cc_toolchain_for_aarch64", +) diff --git a/src/cc_toolchain/BUILD b/src/cc_toolchain/BUILD index 60c869dfed..d5396df343 100644 --- a/src/cc_toolchain/BUILD +++ b/src/cc_toolchain/BUILD @@ -1,46 +1,69 @@ package(default_visibility = ["//visibility:public"]) -load(":cc_toolchain_config.bzl", "cc_toolchain_config_jetson_nano", "cc_toolchain_config_k8") +load( + ":cc_toolchain_config.bzl", + "cc_toolchain_config_fullsystem", + "cc_toolchain_config_k8_jetson_nano_cross_compile", + "make_builtin_include_directories", +) # Create environments for each CPU this toolchain supports environment(name = "k8") -environment(name = "jetson_nano") +environment(name = "k8_jetson_nano_cross_compile") + +environment(name = "aarch64") environment_group( name = "cpus", defaults = [ + ":aarch64", ":k8", - ":jetson_nano", + ":k8_jetson_nano_cross_compile", ], environments = [ + ":aarch64", ":k8", - ":jetson_nano", + ":k8_jetson_nano_cross_compile", ], ) +config_setting( + name = "cpu_aarch64", + values = {"cpu": "aarch64"}, +) + config_setting( name = "cpu_k8", values = {"cpu": "k8"}, ) config_setting( - name = "cpu_jetson_nano", - values = {"cpu": "jetson_nano"}, + name = "cpu_k8_jetson_nano_cross_compile", + values = {"cpu": "k8_jetson_nano_cross_compile"}, +) + +platform( + name = "robot", + constraint_values = [ + "@platforms//cpu:aarch64", + "@platforms//os:linux", + ], ) # This represents a mapping of CPU -> Compiler To Use -# The CPU can be chosen by using `--cpu` as a bazel flag (ex. `--cpu=jetson_nano`) -# otherwise it will default to using whatever CPU is in the system doing the compiling cc_toolchain_suite( name = "toolchain", toolchains = { # k8 is any x86 system - "k8": "cc_toolchain_linux_gcc", - "k8|compiler": "cc_toolchain_linux_gcc", + "k8": "cc_toolchain_linux_k8_gcc", + "k8|compiler": "cc_toolchain_linux_k8_gcc", + # aarch64 is for ARM64 systems and the robot + "aarch64": "cc_toolchain_linux_aarch64_gcc", + "aarch64|compiler": "cc_toolchain_linux_aarch64_gcc", # jetson nano 4GB system - "jetson_nano": "cc_toolchain_jetson_nano", - "jetson_nano|compiler": "cc_toolchain_jetson_nano", + "k8_jetson_nano_cross_compile": "cc_toolchain_k8_jetson_nano_cross_compile", + "k8_jetson_nano_cross_compile|compiler": "cc_toolchain_k8_jetson_nano_cross_compile", }, ) @@ -50,10 +73,10 @@ filegroup( ) filegroup( - name = "jetson_nano_everything", + name = "k8_jetson_nano_cross_compile_everything", srcs = [ - "@jetson_nano_gcc//:everything", - ] + glob(["wrapper/jetson_nano_*"]), + "@k8_jetson_nano_cross_compile_gcc//:everything", + ] + glob(["wrapper/k8_jetson_nano_cross_compile_*"]), ) filegroup( @@ -62,71 +85,95 @@ filegroup( ) filegroup( - name = "linux_gcc_all", + name = "linux_k8_gcc_all", + srcs = [ + ":linux_gcc_wrapper", + "@linux_k8_gcc//:includes", + "@linux_k8_gcc//:libs", + "@linux_k8_gcc//:runtime_libs", + "@linux_k8_gcc//:static_libs", + ], +) + +filegroup( + name = "linux_aarch64_gcc_all", srcs = [ ":linux_gcc_wrapper", - "@linux_gcc//:includes", - "@linux_gcc//:libs", - "@linux_gcc//:runtime_libs", - "@linux_gcc//:static_libs", + "@linux_aarch64_gcc//:includes", + "@linux_aarch64_gcc//:libs", + "@linux_aarch64_gcc//:runtime_libs", + "@linux_aarch64_gcc//:static_libs", ], ) -cc_toolchain_config_k8( - name = "linux_gcc", +cc_toolchain_config_fullsystem( + name = "linux_k8_gcc", builtin_include_directories = [ "/usr/lib/gcc/x86_64-linux-gnu/", - "/usr/include/", - "/usr/local/include/", - "/usr/lib/gcc/c++/*/include/", - ], - cpu = "k8", + ] + make_builtin_include_directories(), + host_system_name = "k8", target_cpu = "k8", target_system_name = "k8", - tool_paths = { - "ar": "/usr/bin/ar", - "cpp": "/usr/bin/cpp", - "dwp": "/usr/bin/dwp", - "gcc": "wrapper/linux_gcc", - "gcov": "/usr/bin/gcov", - "ld": "/usr/bin/ld", - "nm": "/usr/bin/nm", - "objcopy": "/usr/bin/objcopy", - "objdump": "/usr/bin/objdump", - "strip": "/usr/bin/strip", - }, - toolchain_identifier = "linux_gcc-k8", + toolchain_identifier = "linux_k8_gcc", +) + +cc_toolchain_config_fullsystem( + name = "linux_aarch64_gcc", + builtin_include_directories = [ + "/usr/lib/gcc/aarch64-linux-gnu/", + ] + make_builtin_include_directories(), + host_system_name = "aarch64", + target_cpu = "aarch64", + target_system_name = "aarch64", + toolchain_identifier = "linux_aarch64_gcc", ) cc_toolchain( - name = "cc_toolchain_linux_gcc", - all_files = ":linux_gcc_all", - ar_files = ":linux_gcc_all", - as_files = ":linux_gcc_all", - compiler_files = ":linux_gcc_all", - dwp_files = ":linux_gcc_all", - dynamic_runtime_lib = "@linux_gcc//:runtime_libs", - linker_files = ":linux_gcc_all", - objcopy_files = ":linux_gcc_all", - static_runtime_lib = "@linux_gcc//:static_libs", - strip_files = ":linux_gcc_all", + name = "cc_toolchain_linux_k8_gcc", + all_files = ":linux_k8_gcc_all", + ar_files = ":linux_k8_gcc_all", + as_files = ":linux_k8_gcc_all", + compiler_files = ":linux_k8_gcc_all", + dwp_files = ":linux_k8_gcc_all", + dynamic_runtime_lib = "@linux_k8_gcc//:runtime_libs", + linker_files = ":linux_k8_gcc_all", + objcopy_files = ":linux_k8_gcc_all", + static_runtime_lib = "@linux_k8_gcc//:static_libs", + strip_files = ":linux_k8_gcc_all", supports_param_files = True, # We add this tag to circumvent a bug in the CLion bazel plugin. It should be removed # when possible # https://github.com/bazelbuild/intellij/issues/486 tags = ["no-ide"], - toolchain_config = ":linux_gcc", - toolchain_identifier = "k8_gcc-x86_64", + toolchain_config = ":linux_k8_gcc", + toolchain_identifier = "k8_gcc", ) cc_toolchain( - name = "cc_toolchain_jetson_nano", - all_files = ":jetson_nano_everything", - ar_files = ":jetson_nano_everything", - as_files = ":jetson_nano_everything", - compiler_files = ":jetson_nano_everything", + name = "cc_toolchain_linux_aarch64_gcc", + all_files = ":linux_aarch64_gcc_all", + ar_files = ":linux_aarch64_gcc_all", + as_files = ":linux_aarch64_gcc_all", + compiler_files = ":linux_aarch64_gcc_all", + dwp_files = ":linux_aarch64_gcc_all", + dynamic_runtime_lib = "@linux_aarch64_gcc//:runtime_libs", + linker_files = ":linux_aarch64_gcc_all", + objcopy_files = ":linux_aarch64_gcc_all", + static_runtime_lib = "@linux_aarch64_gcc//:static_libs", + strip_files = ":linux_aarch64_gcc_all", + supports_param_files = True, + toolchain_config = ":linux_aarch64_gcc", + toolchain_identifier = "aarch64_gcc", +) + +cc_toolchain( + name = "cc_toolchain_k8_jetson_nano_cross_compile", + all_files = ":k8_jetson_nano_cross_compile_everything", + ar_files = ":k8_jetson_nano_cross_compile_everything", + as_files = ":k8_jetson_nano_cross_compile_everything", + compiler_files = ":k8_jetson_nano_cross_compile_everything", dwp_files = ":empty", - linker_files = ":jetson_nano_everything", + linker_files = ":k8_jetson_nano_cross_compile_everything", objcopy_files = ":empty", strip_files = ":empty", supports_param_files = 0, @@ -134,30 +181,71 @@ cc_toolchain( # when possible # https://github.com/bazelbuild/intellij/issues/486 tags = ["no-ide"], - toolchain_config = ":gcc-jetson_nano", - toolchain_identifier = "gcc-jetson_nano", + toolchain_config = ":gcc_k8_jetson_nano_cross_compile", + toolchain_identifier = "gcc-k8-jetson-nano-cross-compile", ) -cc_toolchain_config_jetson_nano( - name = "gcc-jetson_nano", +cc_toolchain_config_k8_jetson_nano_cross_compile( + name = "gcc_k8_jetson_nano_cross_compile", builtin_include_directories = [ - "external/jetson_nano_gcc/include/", - "external/jetson_nano_gcc/aarch64-linux-gnu/include/", - "external/jetson_nano_gcc/aarch64-linux-gnu/libc/lib/", - "external/jetson_nano_gcc/aarch64-linux-gnu/libc/usr/include/", + "external/k8_jetson_nano_cross_compile_gcc/include/", + "external/k8_jetson_nano_cross_compile_gcc/aarch64-linux-gnu/include/", + "external/k8_jetson_nano_cross_compile_gcc/aarch64-linux-gnu/libc/lib/", + "external/k8_jetson_nano_cross_compile_gcc/aarch64-linux-gnu/libc/usr/include/", ], - cpu = "jetson_nano", tool_paths = { - "ar": "wrapper/jetson_nano_ar", - "cpp": "wrapper/jetson_nano_cpp", - "dwp": "wrapper/jetson_nano_dwp", - "gcc": "wrapper/jetson_nano_gcc", - "gcov": "wrapper/jetson_nano_gcov", - "ld": "wrapper/jetson_nano_ld", - "nm": "wrapper/jetson_nano_nm", - "objcopy": "wrapper/jetson_nano_objcopy", - "objdump": "wrapper/jetson_nano_objdump", - "strip": "wrapper/jetson_nano_strip", + "ar": "wrapper/k8_jetson_nano_cross_compile_ar", + "cpp": "wrapper/k8_jetson_nano_cross_compile_cpp", + "dwp": "wrapper/k8_jetson_nano_cross_compile_dwp", + "gcc": "wrapper/k8_jetson_nano_cross_compile_gcc", + "gcov": "wrapper/k8_jetson_nano_cross_compile_gcov", + "ld": "wrapper/k8_jetson_nano_cross_compile_ld", + "nm": "wrapper/k8_jetson_nano_cross_compile_nm", + "objcopy": "wrapper/k8_jetson_nano_cross_compile_objcopy", + "objdump": "wrapper/k8_jetson_nano_cross_compile_objdump", + "strip": "wrapper/k8_jetson_nano_cross_compile_strip", }, - toolchain_identifier = "gcc-jetson_nano", + toolchain_identifier = "gcc-k8-jetson_nano-cross-compile", +) + +toolchain( + name = "cc_toolchain_for_k8_jetson_nano_cross_compile", + exec_compatible_with = [ + "@platforms//cpu:x86_64", + "@platforms//os:linux", + ], + target_compatible_with = [ + "@platforms//cpu:aarch64", + "@platforms//os:linux", + ], + toolchain = ":cc_toolchain_k8_jetson_nano_cross_compile", + toolchain_type = "@bazel_tools//tools/cpp:toolchain_type", +) + +toolchain( + name = "cc_toolchain_for_k8", + exec_compatible_with = [ + "@platforms//os:linux", + "@platforms//cpu:x86_64", + ], + target_compatible_with = [ + "@platforms//os:linux", + "@platforms//cpu:x86_64", + ], + toolchain = ":cc_toolchain_linux_k8_gcc", + toolchain_type = "@bazel_tools//tools/cpp:toolchain_type", +) + +toolchain( + name = "cc_toolchain_for_aarch64", + exec_compatible_with = [ + "@platforms//cpu:aarch64", + "@platforms//os:linux", + ], + target_compatible_with = [ + "@platforms//cpu:aarch64", + "@platforms//os:linux", + ], + toolchain = ":cc_toolchain_linux_aarch64_gcc", + toolchain_type = "@bazel_tools//tools/cpp:toolchain_type", ) diff --git a/src/cc_toolchain/cc_toolchain_config.bzl b/src/cc_toolchain/cc_toolchain_config.bzl index 00f9e08373..1544046401 100644 --- a/src/cc_toolchain/cc_toolchain_config.bzl +++ b/src/cc_toolchain/cc_toolchain_config.bzl @@ -104,6 +104,27 @@ ALL_CPP_ACTIONS = [ ACTION_NAMES.cpp_link_nodeps_dynamic_library, ] +def make_builtin_include_directories(): + return [ + "/usr/include/", + "/usr/local/include/", + "/usr/lib/gcc/c++/*/include/", + ] + +def _make_common_toolpaths(): + return { + "ar": "/usr/bin/ar", + "cpp": "/usr/bin/cpp", + "dwp": "/usr/bin/dwp", + "gcc": "wrapper/linux_gcc", + "gcov": "/usr/bin/gcov", + "ld": "/usr/bin/ld", + "nm": "/usr/bin/nm", + "objcopy": "/usr/bin/objcopy", + "objdump": "/usr/bin/objdump", + "strip": "/usr/bin/strip", + } + def _make_common_features(ctx): result = {} @@ -401,13 +422,11 @@ def _make_common_features(ctx): return result def _linux_gcc_impl(ctx): - host_system_name = "k8" - action_configs = [] tool_paths = [ tool_path(name = name, path = path) - for name, path in ctx.attr.tool_paths.items() + for name, path in _make_common_toolpaths().items() ] common = _make_common_features(ctx) @@ -560,7 +579,7 @@ def _linux_gcc_impl(ctx): artifact_name_patterns = [], cxx_builtin_include_directories = ctx.attr.builtin_include_directories, toolchain_identifier = ctx.attr.toolchain_identifier, - host_system_name = host_system_name, + host_system_name = ctx.attr.host_system_name, target_system_name = ctx.attr.target_system_name, target_cpu = ctx.attr.target_cpu, target_libc = "libc", @@ -577,14 +596,14 @@ def _linux_gcc_impl(ctx): ), ] -cc_toolchain_config_k8 = rule( +cc_toolchain_config_fullsystem = rule( implementation = _linux_gcc_impl, attrs = { "builtin_include_directories": attr.string_list(), - "cpu": attr.string(mandatory = True, values = ["k8"]), "extra_features": attr.string_list(), "extra_no_canonical_prefixes_flags": attr.string_list(), "host_compiler_warnings": attr.string_list(), + "host_system_name": attr.string(), "host_unfiltered_compile_flags": attr.string_list(), "target_cpu": attr.string(), "target_system_name": attr.string(), @@ -595,7 +614,7 @@ cc_toolchain_config_k8 = rule( executable = True, ) -def _jetson_nano_impl(ctx): +def _k8_jetson_nano_cross_compile_impl(ctx): host_system_name = "k8" action_configs = [] @@ -708,12 +727,9 @@ def _jetson_nano_impl(ctx): ), ] -cc_toolchain_config_jetson_nano = rule( - implementation = _jetson_nano_impl, +cc_toolchain_config_k8_jetson_nano_cross_compile = rule( + implementation = _k8_jetson_nano_cross_compile_impl, attrs = { - "cpu": attr.string(mandatory = True, values = [ - "jetson_nano", - ]), "builtin_include_directories": attr.string_list(), "extra_no_canonical_prefixes_flags": attr.string_list(), "host_compiler_warnings": attr.string_list(), diff --git a/src/cc_toolchain/wrapper/jetson_nano_ar b/src/cc_toolchain/wrapper/jetson_nano_ar deleted file mode 100755 index d40a860b6e..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_ar +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-ar "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_cpp b/src/cc_toolchain/wrapper/jetson_nano_cpp deleted file mode 100755 index aff3764d78..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_cpp +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-cpp "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_g++ b/src/cc_toolchain/wrapper/jetson_nano_g++ deleted file mode 100755 index b8f943f7e3..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_g++ +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-g++ -Wl,--no-as-needed "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_gcc b/src/cc_toolchain/wrapper/jetson_nano_gcc deleted file mode 100755 index 64774504ab..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_gcc +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-gcc -Wl,--no-as-needed "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_gcov b/src/cc_toolchain/wrapper/jetson_nano_gcov deleted file mode 100755 index 7922e92416..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_gcov +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-gcov "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_ld b/src/cc_toolchain/wrapper/jetson_nano_ld deleted file mode 100755 index 51728eeb07..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_ld +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-ld "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_nm b/src/cc_toolchain/wrapper/jetson_nano_nm deleted file mode 100755 index 98c9a6ba28..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_nm +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-nm "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_objcopy b/src/cc_toolchain/wrapper/jetson_nano_objcopy deleted file mode 100755 index 70ff1f0e61..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_objcopy +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-objcopy "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_objdump b/src/cc_toolchain/wrapper/jetson_nano_objdump deleted file mode 100755 index b17ce50c8c..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_objdump +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-objdump "$@" diff --git a/src/cc_toolchain/wrapper/jetson_nano_strip b/src/cc_toolchain/wrapper/jetson_nano_strip deleted file mode 100755 index 1d67bf46c0..0000000000 --- a/src/cc_toolchain/wrapper/jetson_nano_strip +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash --norc - -exec external/jetson_nano_gcc/bin/aarch64-linux-gnu-strip "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar new file mode 100755 index 0000000000..233d5e3c4c --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ar @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-ar "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp new file mode 100755 index 0000000000..8977e9e4fd --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_cpp @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-cpp "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ new file mode 100755 index 0000000000..a3f3025445 --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_g++ @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-g++ -Wl,--no-as-needed "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc new file mode 100755 index 0000000000..cad72ed459 --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcc @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-gcc -Wl,--no-as-needed "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcov b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcov new file mode 100755 index 0000000000..7c3616749d --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_gcov @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-gcov "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld new file mode 100755 index 0000000000..7ed51ae2b3 --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_ld @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-ld "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm new file mode 100755 index 0000000000..2eb52826c7 --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_nm @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-nm "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objcopy b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objcopy new file mode 100755 index 0000000000..2b324ad0cc --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objcopy @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-objcopy "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objdump b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objdump new file mode 100755 index 0000000000..1b69911e81 --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_objdump @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-objdump "$@" diff --git a/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_strip b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_strip new file mode 100755 index 0000000000..67a975cc89 --- /dev/null +++ b/src/cc_toolchain/wrapper/k8_jetson_nano_cross_compile_strip @@ -0,0 +1,3 @@ +#!/bin/bash --norc + +exec external/k8_jetson_nano_cross_compile_gcc/bin/aarch64-linux-gnu-strip "$@" diff --git a/src/cc_toolchain/wrapper/linux_gcc b/src/cc_toolchain/wrapper/linux_gcc index 9c27b70983..e0800abb9c 100755 --- a/src/cc_toolchain/wrapper/linux_gcc +++ b/src/cc_toolchain/wrapper/linux_gcc @@ -6,4 +6,4 @@ # only link in the direct dependencies for each shared lib, but because # bazel delays linking to the end of the build, this means some (non-direct) # dependencies of the final target will not be linked in -exec /usr/bin/gcc-9 -Wl,--no-as-needed "$@" +exec /usr/bin/gcc-10 -Wl,--no-as-needed "$@" diff --git a/src/extlibs/er_force_sim/README.md b/src/extlibs/er_force_sim/README.md index c6a65ed338..257a2e5dbe 100644 --- a/src/extlibs/er_force_sim/README.md +++ b/src/extlibs/er_force_sim/README.md @@ -1,7 +1,11 @@ # The Simulator from Roboterfußballmannschaft der Friedrich-Alexander-Universität Erlangen-Nürnberg Robotics Erlangen's SSL-Team ER-Force's Framework -The simulator was adapted from [robotics-erlangen/framework](https://github.com/robotics-erlangen/framework). It was modified so that time steps could be controlled by a stepSimulation function. + +The simulator is adapted from [robotics-erlangen/framework](https://github.com/robotics-erlangen/framework). It contains several bug fixes and general code quality improvements, and it is modified so that time steps can be controlled by a `stepSimulation` function. + ## Copyright + +``` /*************************************************************************** * Copyright 2020 Michael Eischer, Philipp Nordhus, Andreas Wendler * * Robotics Erlangen e.V. * @@ -21,3 +25,4 @@ The simulator was adapted from [robotics-erlangen/framework](https://github.com/ * You should have received a copy of the GNU General Public License * * along with this program. If not, see . * ***************************************************************************/ +``` diff --git a/src/extlibs/er_force_sim/src/amun/simulator/BUILD b/src/extlibs/er_force_sim/src/amun/simulator/BUILD index 76473358a4..69a9a7b732 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/BUILD +++ b/src/extlibs/er_force_sim/src/amun/simulator/BUILD @@ -1,21 +1,15 @@ -load("@bazel_rules_qt//:qt.bzl", "qt_cc_library") - package(default_visibility = ["//visibility:public"]) -qt_cc_library( - name = "simulator_qt", +cc_library( + name = "simulator", srcs = glob(["*.cpp"]), hdrs = glob(["*.h"]), deps = [ - "//extlibs/er_force_sim/src/core:core_qt", - "//extlibs/er_force_sim/src/protobuf:protobuf_qt", + "//extlibs/er_force_sim/src/core", + "//extlibs/er_force_sim/src/protobuf", "//proto:ssl_simulation_cc_proto", "//shared:constants", "@bullet", - "@qt//:qt_core", - "@qt//:qt_gui", - "@qt//:qt_widgets", ], - #linkstatic = True, alwayslink = True, ) diff --git a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.cpp b/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.cpp deleted file mode 100644 index 850e63c24d..0000000000 --- a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * Copyright 2021 Tobias Heineken * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#include "erroraggregator.h" - -#include - -#include "simulator.h" - -using namespace camun::simulator; - -void ErrorAggregator::aggregate(SSLSimError error, ErrorSource source) -{ - m_data[source].push_back(std::move(error)); -} - -QList ErrorAggregator::getAggregates(ErrorSource source) -{ - QList &ref = m_data[source]; - QList out = ref; - ref.clear(); - return out; -} diff --git a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.h b/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.h deleted file mode 100644 index 7f10d3e39d..0000000000 --- a/src/extlibs/er_force_sim/src/amun/simulator/erroraggregator.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * Copyright 2021 Tobias Heineken * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#ifndef SIM_AGGREGATOR_H -#define SIM_AGGREGATOR_H -#include -#include - -#include "extlibs/er_force_sim/src/protobuf/sslsim.h" - -namespace camun -{ - namespace simulator - { - enum class ErrorSource; - class ErrorAggregator : public QObject - { - Q_OBJECT - public: - ErrorAggregator(QObject* parent) : QObject(parent) {} - - public slots: - void aggregate(SSLSimError eror, ErrorSource e); - - public: - QList getAggregates(ErrorSource e); - - private: - QMap> m_data; - }; - } // namespace simulator -} // namespace camun -#endif diff --git a/src/extlibs/er_force_sim/src/amun/simulator/mesh.cpp b/src/extlibs/er_force_sim/src/amun/simulator/mesh.cpp deleted file mode 100644 index 599090fe94..0000000000 --- a/src/extlibs/er_force_sim/src/amun/simulator/mesh.cpp +++ /dev/null @@ -1,96 +0,0 @@ -/*************************************************************************** - * Copyright 2015 Michael Eischer, Philipp Nordhus * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#include "mesh.h" - -#include - -using namespace camun::simulator; - -/*! - * \class Mesh - * \ingroup simulator - * \brief A 3D mesh - */ - -Mesh::Mesh(float radius, float height, float angle, float holeSize, float boxHeight) - : m_radius(radius), m_height(height), m_angle(angle), m_holeSize(holeSize) -{ - const float frontPlateLength = std::sin(angle / 2.0f) * radius; - const float frontPlatePos = radius * std::cos(angle / 2.0f); - const float holePlatePos = frontPlatePos - holeSize; - const float outerAngle = std::acos(holePlatePos / radius) * 2; - const float angleDiff = (outerAngle - angle) / 2.0f; - const float halfOuterAngle = outerAngle / 2.0f; - const float outerAngleStart = halfOuterAngle + M_PI_2; - const float outerAngleStop = 2.0f * M_PI - halfOuterAngle + M_PI_2; - addRobotCover(20, outerAngleStart, outerAngleStop); - - // right pillar - addRobotCover(5, outerAngleStart - angleDiff, outerAngleStart); - m_hull.back().append(QVector3D(-frontPlateLength, holePlatePos, m_height / 2.0f)); - m_hull.back().append(QVector3D(-frontPlateLength, holePlatePos, -m_height / 2.0f)); - - // left pillar - addRobotCover(5, outerAngleStop, outerAngleStop + angleDiff); - m_hull.back().append(QVector3D(frontPlateLength, holePlatePos, m_height / 2.0f)); - m_hull.back().append(QVector3D(frontPlateLength, holePlatePos, -m_height / 2.0f)); - - // the remaining box - QList boxPart; - boxPart.append(QVector3D(frontPlateLength, holePlatePos, m_height / 2.0f)); - boxPart.append(QVector3D(-frontPlateLength, holePlatePos, m_height / 2.0f)); - boxPart.append( - QVector3D(frontPlateLength, holePlatePos, -m_height / 2.0f + boxHeight)); - boxPart.append( - QVector3D(-frontPlateLength, holePlatePos, -m_height / 2.0f + boxHeight)); - boxPart.append(QVector3D(frontPlateLength, frontPlatePos, m_height / 2.0f)); - boxPart.append(QVector3D(-frontPlateLength, frontPlatePos, m_height / 2.0f)); - boxPart.append( - QVector3D(frontPlateLength, frontPlatePos, -m_height / 2.0f + boxHeight)); - boxPart.append( - QVector3D(-frontPlateLength, frontPlatePos, -m_height / 2.0f + boxHeight)); - m_hull.append(boxPart); -} - -/*! - * \brief Adds triangles for the outer hull - * \param radius Radius of the robot - * \param height Height of the robot - * \param num Number of segments - * \param angle Angle of the hull start - * \param angleStep Step size in rad - */ -void Mesh::addRobotCover(uint num, float startAngle, float endAngle) -{ - QList covers; - float angle = startAngle; - float angleStep = (endAngle - startAngle) / num; - for (uint i = 0; i <= num; i++) - { - covers.append( - QVector3D(m_radius * cos(angle), m_radius * sin(angle), m_height / 2.0f)); - covers.append( - QVector3D(m_radius * cos(angle), m_radius * sin(angle), -m_height / 2.0f)); - - angle += angleStep; - } - m_hull.append(covers); -} diff --git a/src/extlibs/er_force_sim/src/amun/simulator/mesh.h b/src/extlibs/er_force_sim/src/amun/simulator/mesh.h deleted file mode 100644 index e25ad2c019..0000000000 --- a/src/extlibs/er_force_sim/src/amun/simulator/mesh.h +++ /dev/null @@ -1,60 +0,0 @@ -/*************************************************************************** - * Copyright 2015 Michael Eischer, Philipp Nordhus * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#ifndef MESH_H -#define MESH_H - -#include -#include -#include -#include - -namespace camun -{ - namespace simulator - { - class Mesh; - } -} // namespace camun - -class camun::simulator::Mesh -{ - public: - Mesh(float radius, float height, float angle, float holeSize, float boxHeight); - void createRobotMeshes(float radius, float height, float angle); - const QList> &hull() const - { - return m_hull; - } - - private: - void addRobotCover(uint num, float startAngle, float endAngle); - void addSidePart(uint num, float angleStart, float angleStop, bool right); - - private: - QList> m_hull; - - const float m_radius; - const float m_height; - const float m_angle; - const float m_holeSize; -}; - -#endif // MESH_H diff --git a/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.cpp b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.cpp new file mode 100644 index 0000000000..d06d66390c --- /dev/null +++ b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.cpp @@ -0,0 +1,135 @@ +/*************************************************************************** + * Copyright 2015 Michael Eischer, Philipp Nordhus * + * Robotics Erlangen e.V. * + * http://www.robotics-erlangen.de/ * + * info@robotics-erlangen.de * + * * + * This program is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program. If not, see . * + ***************************************************************************/ + +#include "robotmesh.h" + +#include + +using namespace camun::simulator; + +static std::vector> generateRobotShellPoints( + unsigned int numSegments, float startAngle, float endAngle, float radius, + float height) +{ + std::vector> points; + + float angle = startAngle; + const float angleStep = (endAngle - startAngle) / numSegments; + + for (unsigned int i = 0; i <= numSegments; ++i) + { + const float x = radius * std::cos(angle); + const float y = radius * std::sin(angle); + + points.emplace_back(x, y, height / 2.0); + points.emplace_back(x, y, -height / 2.0); + + angle += angleStep; + } + + return points; +} + +std::vector>> +camun::simulator::createRobotMesh(float radius, float height, float angle, + float holeDepth, float holeHeight) +{ + static constexpr unsigned int NUM_SEGMENTS_HULL = 20; + static constexpr unsigned int NUM_SEGMENTS_PILLAR = 5; + + // Diagram of robot (top view) + // + // ┌────────┬────frontPlatePos + // ┌───┬─────────holePlatePos + // ┌────┬────holeDepth + // , - ~ - , + // , ' . ' , + // , . .| ─┐ + // , . . | │ + // , .\. | │ + // , + )─┐ | ├─frontPlateLength + // , `/` │ | │ + // , │ │' | │ + // , │` │ .| │ + // , │ `│ , ' ─┘ + // ' - , _│, │' + // │ └─angle + // └────angleDiff + // outerAngle = angle + angleDiff + angleDiff + // + const float frontPlateLength = std::sin(angle / 2.0) * radius; + const float frontPlatePos = radius * std::cos(angle / 2.0); + const float holePlatePos = frontPlatePos - holeDepth; + const float outerAngle = std::acos(holePlatePos / radius) * 2; + const float angleDiff = (outerAngle - angle) / 2.0; + const float halfOuterAngle = outerAngle / 2.0; + const float outerAngleStart = halfOuterAngle + M_PI_2; + const float outerAngleStop = 2.0 * M_PI - halfOuterAngle + M_PI_2; + + std::vector>> meshParts; + + // Parts of robot (top view) + // + // , - ~ - , Right pillar + // , ' | ' , + // , ├────┐ + // , | | + // , | | + // , Main hull | Front plate and dribbler hole + // , | | + // , | | + // , ├────┘ + // , | .' + // ' - , _ , ' Left pillar + // + + // Main hull + meshParts.push_back(generateRobotShellPoints(NUM_SEGMENTS_HULL, outerAngleStart, + outerAngleStop, radius, height)); + + // Left pillar + auto leftPillar = generateRobotShellPoints( + NUM_SEGMENTS_PILLAR, outerAngleStop, outerAngleStop + angleDiff, radius, height); + leftPillar.emplace_back(frontPlateLength, holePlatePos, height / 2.0); + leftPillar.emplace_back(frontPlateLength, holePlatePos, -height / 2.0); + meshParts.push_back(leftPillar); + + // Right pillar + auto rightPillar = + generateRobotShellPoints(NUM_SEGMENTS_PILLAR, outerAngleStart - angleDiff, + outerAngleStart, radius, height); + rightPillar.emplace_back(-frontPlateLength, holePlatePos, height / 2.0); + rightPillar.emplace_back(-frontPlateLength, holePlatePos, -height / 2.0); + meshParts.push_back(rightPillar); + + // The remaining front plate and dribbler "hole" + meshParts.push_back({ + {frontPlateLength, holePlatePos, height / 2.0}, + {-frontPlateLength, holePlatePos, height / 2.0}, + {frontPlateLength, holePlatePos, -height / 2.0 + holeHeight}, + {-frontPlateLength, holePlatePos, -height / 2.0 + holeHeight}, + {frontPlateLength, frontPlatePos, height / 2.0}, + {-frontPlateLength, frontPlatePos, height / 2.0}, + {frontPlateLength, frontPlatePos, -height / 2.0 + holeHeight}, + {-frontPlateLength, frontPlatePos, -height / 2.0 + holeHeight}, + }); + + return meshParts; +} diff --git a/src/extlibs/er_force_sim/src/protobuf/sslsim.h b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h similarity index 76% rename from src/extlibs/er_force_sim/src/protobuf/sslsim.h rename to src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h index 8bdc8aac12..3670e3f81e 100644 --- a/src/extlibs/er_force_sim/src/protobuf/sslsim.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h @@ -1,5 +1,5 @@ /*************************************************************************** - * Copyright 2021 Tobias Heineken * + * Copyright 2015 Michael Eischer, Philipp Nordhus * * Robotics Erlangen e.V. * * http://www.robotics-erlangen.de/ * * info@robotics-erlangen.de * @@ -18,14 +18,19 @@ * along with this program. If not, see . * ***************************************************************************/ -#ifndef PB_SSLSIM_H -#define PB_SSLSIM_H +#ifndef ROBOTMESH_H +#define ROBOTMESH_H -#include +#include +#include -#include "extlibs/er_force_sim/src/protobuf/ssl_simulation_error.pb.h" -#include "extlibs/er_force_sim/src/protobuf/ssl_simulation_robot_control.pb.h" +namespace camun +{ + namespace simulator + { + std::vector>> createRobotMesh( + float radius, float height, float angle, float holeDepth, float holeHeight); + } // namespace simulator +} // namespace camun -typedef QSharedPointer SSLSimRobotControl; -typedef QSharedPointer SSLSimError; -#endif +#endif // ROBOTMESH_H diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp index 054a1d8b24..81c40e84c3 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simball.cpp @@ -20,30 +20,26 @@ #include "simball.h" -#include #include #include "extlibs/er_force_sim/src/core/coordinates.h" -#include "extlibs/er_force_sim/src/core/rng.h" #include "extlibs/er_force_sim/src/core/vector.h" -#include "proto/ssl_vision_detection.pb.h" #include "simulator.h" using namespace camun::simulator; -SimBall::SimBall(RNG *rng, btDiscreteDynamicsWorld *world) - : m_rng(rng), - m_world(world), - current_ball_state(STATIONARY), - set_transition_speed(true), - rolling_speed(-1.0f) +SimBall::SimBall(std::shared_ptr world) + : m_world(world), + m_rolling_speed(-1.0f), + m_set_transition_speed(true), + m_current_ball_state(STATIONARY) { // see http://robocup.mi.fu-berlin.de/buch/rolling.pdf for correct modelling - m_sphere = - new btSphereShape(static_cast(BALL_MAX_RADIUS_METERS) * SIMULATOR_SCALE); + m_sphere = std::make_unique( + static_cast(BALL_MAX_RADIUS_METERS) * SIMULATOR_SCALE); - btVector3 localInertia(0, 0, 0); // FIXME measure inertia coefficient + btVector3 localInertia(0, 0, 0); m_sphere->calculateLocalInertia(BALL_MASS_KG, localInertia); btTransform startWorldTransform; @@ -51,30 +47,28 @@ SimBall::SimBall(RNG *rng, btDiscreteDynamicsWorld *world) startWorldTransform.setOrigin( btVector3(0.0f, 0.0f, static_cast(BALL_MAX_RADIUS_METERS)) * SIMULATOR_SCALE); - m_motionState = new btDefaultMotionState(startWorldTransform); + m_motionState = std::make_unique(startWorldTransform); - btRigidBody::btRigidBodyConstructionInfo rbInfo(BALL_MASS_KG, m_motionState, m_sphere, - localInertia); + btRigidBody::btRigidBodyConstructionInfo rbInfo(BALL_MASS_KG, m_motionState.get(), + m_sphere.get(), localInertia); // parameters seem to be ignored... - m_body = new btRigidBody(rbInfo); - // see simulator.cpp - // TODO (#2512): Check these values with real life + m_body = std::make_unique(rbInfo); + + // See simulator.h m_body->setRestitution(BALL_RESTITUTION); m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS); // \mu_r = -a / g = 0.0357 (while rolling) // rollingFriction in bullet is too unstable to be useful // use custom implementation in begin() - m_world->addRigidBody(m_body); + + m_world->addRigidBody(m_body.get()); } SimBall::~SimBall() { - m_world->removeRigidBody(m_body); - delete m_body; - delete m_sphere; - delete m_motionState; + m_world->removeRigidBody(m_body.get()); } void SimBall::begin(bool robot_collision) @@ -86,48 +80,47 @@ void SimBall::begin(bool robot_collision) { // ball is on the ground bool is_stationary = velocity.length() < STATIONARY_BALL_SPEED_METERS_PER_SECOND * SIMULATOR_SCALE; - bool should_roll = velocity.length() < rolling_speed * SIMULATOR_SCALE; + bool should_roll = velocity.length() < m_rolling_speed * SIMULATOR_SCALE; if (robot_collision) { - current_ball_state = ROBOT_COLLISION; + m_current_ball_state = ROBOT_COLLISION; } else if (is_stationary) { - current_ball_state = STATIONARY; + m_current_ball_state = STATIONARY; } - else if ((current_ball_state == SLIDING && should_roll) || - current_ball_state == ROLLING) + else if ((m_current_ball_state == SLIDING && should_roll) || + m_current_ball_state == ROLLING) { - current_ball_state = ROLLING; + m_current_ball_state = ROLLING; } else { - current_ball_state = SLIDING; + m_current_ball_state = SLIDING; } - switch (current_ball_state) + switch (m_current_ball_state) { case STATIONARY: m_body->setLinearVelocity(btVector3(0, 0, 0)); m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS); - set_transition_speed = true; + m_set_transition_speed = true; break; case ROBOT_COLLISION: m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS); - set_transition_speed = true; + m_set_transition_speed = true; break; case SLIDING: m_body->setFriction(BALL_SLIDING_FRICTION_NEWTONS); - if (set_transition_speed) + if (m_set_transition_speed) { - rolling_speed = + m_rolling_speed = FRICTION_TRANSITION_FACTOR * velocity.length() / SIMULATOR_SCALE; } - set_transition_speed = false; + m_set_transition_speed = false; break; case ROLLING: - // just apply rolling friction, normal friction is somehow handled by // bullet const btScalar rollingDeceleration = @@ -139,54 +132,32 @@ void SimBall::begin(bool robot_collision) SUB_TIMESTEP); m_body->setFriction(0.0f); - set_transition_speed = false; + m_set_transition_speed = false; break; } } - bool moveCommand = false; - auto sendPartialCoordError = [this](const char *msg) { - SSLSimError error{new sslsim::SimulatorError}; - error->set_code("PARTIAL_COORD"); - std::string message = "Partial coordinates are not implemented yet"; - error->set_message(message + msg); - emit this->sendSSLSimError(error, ErrorSource::CONFIG); - }; - if (m_move.has_x()) + if (m_move.has_x() != m_move.has_y()) { - if (!m_move.has_y()) - { - sendPartialCoordError(": position ball"); - return; - } - moveCommand = true; + std::cerr << "Partial coordinates are not implemented yet: ball position" + << std::endl; + return; } - else if (m_move.has_y() || m_move.has_z()) + + if (m_move.has_vx() != m_move.has_vy()) { - sendPartialCoordError(": position ball (not x)"); + std::cerr << "Partial coordinates are not implemented yet: ball velocity" + << std::endl; return; } - if (m_move.has_vx()) + if (m_move.by_force() && (m_move.vx() != 0 || m_move.vy() != 0 || m_move.vz() != 0)) { - if (!m_move.has_vy()) - { - sendPartialCoordError(": velocity ball"); - return; - } - if (m_move.by_force() && (m_move.vx() != 0 || m_move.vy() != 0 || - (m_move.has_vz() && m_move.vz() != 0))) - { - SSLSimError error{new sslsim::SimulatorError}; - error->set_code("VELOCITY_FORCE"); - error->set_message("Velocities != 0 and by_force are incompatible"); - emit sendSSLSimError(error, ErrorSource::CONFIG); - return; - } - moveCommand = true; + std::cerr << "Velocities != 0 and by_force are incompatible" << std::endl; + return; } - if (moveCommand) + if (m_move.has_x() || m_move.has_vx()) { if (m_move.by_force()) { @@ -232,9 +203,9 @@ void SimBall::begin(bool robot_collision) m_body->setLinearVelocity(linVel * SIMULATOR_SCALE); // override ballState - current_ball_state = SLIDING; - rolling_speed = linVel.length() * FRICTION_TRANSITION_FACTOR; - set_transition_speed = false; + m_current_ball_state = SLIDING; + m_rolling_speed = linVel.length() * FRICTION_TRANSITION_FACTOR; + m_set_transition_speed = false; m_body->setAngularVelocity(btVector3(0, 0, 0)); } @@ -328,7 +299,7 @@ static float positionOfVisiblePixels(btVector3 &p, const btVector3 &simulatorBal return static_cast(cameraHitCounter) / static_cast(maxHits); } -bool SimBall::update(SSLProto::SSL_DetectionBall *ball, float stddev, float stddevArea, +bool SimBall::update(SSLProto::SSL_DetectionBall &ball, float stddev, float stddevArea, const btVector3 &cameraPosition, bool enableInvisibleBall, float visibilityThreshold, btVector3 positionOffset) { @@ -340,15 +311,15 @@ bool SimBall::update(SSLProto::SSL_DetectionBall *ball, float stddev, float stdd enableInvisibleBall, visibilityThreshold, positionOffset); } -bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, float stddev, +bool SimBall::addDetection(SSLProto::SSL_DetectionBall &ball, btVector3 pos, float stddev, float stddevArea, const btVector3 &cameraPosition, bool enableInvisibleBall, float visibilityThreshold, btVector3 positionOffset) { // setup ssl-vision ball detection - ball->set_confidence(1.0f); - ball->set_pixel_x(0.0f); - ball->set_pixel_y(0.0f); + ball.set_confidence(1.0f); + ball.set_pixel_x(0.0f); + ball.set_pixel_y(0.0f); btTransform transform; m_motionState->getWorldTransform(transform); @@ -365,7 +336,7 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo { // if the visibility is lower than the threshold the ball disappears visibility = positionOfVisiblePixels(pos, transform.getOrigin(), - simulatorCameraPosition, m_world); + simulatorCameraPosition, m_world.get()); if (visibility < visibilityThreshold) { return false; @@ -398,21 +369,15 @@ bool SimBall::addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, flo float area = visibility * std::max(0.0f, (basePixelArea + - static_cast(m_rng->normal(stddevArea)) / PIXEL_PER_AREA)); - ball->set_area(area * PIXEL_PER_AREA); - - // if (height > 0.1f) { - // qDebug() << "simball" << p.x() << p.y() << height << "ttt" << ball_x << - // ball_y; - // } + static_cast(m_rng.normal(stddevArea)) / PIXEL_PER_AREA)); + ball.set_area(area * PIXEL_PER_AREA); // add noise to coordinates - // to convert from bullet coordinate system to ssl-vision rotate by 90 degree - // ccw - const ErForceVector noise = m_rng->normalVector(stddev); - coordinates::toVision(ErForceVector(modX, modY) + noise, *ball); + // to convert from bullet coordinate system to ssl-vision rotate by 90 degree ccw + const ErForceVector noise = m_rng.normalVector(stddev); + coordinates::toVision(ErForceVector(modX, modY) + noise, ball); - ball->set_z(modZ * 1000); // modZ is in kilometres, need to convert to metres + ball.set_z(modZ * 1000); // modZ is in kilometres, need to convert to metres return true; } @@ -433,21 +398,21 @@ btVector3 SimBall::speed() const return m_body->getLinearVelocity(); } -void SimBall::writeBallState(world::SimBall *ball) const +void SimBall::writeBallState(world::SimBall &ball) const { const btVector3 ballPosition = m_body->getWorldTransform().getOrigin() / SIMULATOR_SCALE; - ball->set_p_x(ballPosition.getX()); - ball->set_p_y(ballPosition.getY()); - ball->set_p_z(ballPosition.getZ()); + ball.set_p_x(ballPosition.getX()); + ball.set_p_y(ballPosition.getY()); + ball.set_p_z(ballPosition.getZ()); const btVector3 ballSpeed = speed() / SIMULATOR_SCALE; - ball->set_v_x(ballSpeed.getX()); - ball->set_v_y(ballSpeed.getY()); - ball->set_v_z(ballSpeed.getZ()); + ball.set_v_x(ballSpeed.getX()); + ball.set_v_y(ballSpeed.getY()); + ball.set_v_z(ballSpeed.getZ()); const btVector3 angularVelocity = m_body->getAngularVelocity(); - ball->set_angular_x(angularVelocity.x()); - ball->set_angular_y(angularVelocity.y()); - ball->set_angular_z(angularVelocity.z()); + ball.set_angular_x(angularVelocity.x()); + ball.set_angular_y(angularVelocity.y()); + ball.set_angular_z(angularVelocity.z()); } void SimBall::restoreState(const world::SimBall &ball) @@ -479,8 +444,4 @@ void SimBall::kick(const btVector3 &power) { m_body->activate(); m_body->applyCentralForce(power); - // btTransform transform; - // m_motionState->getWorldTransform(transform); - // const btVector3 p = transform.getOrigin() / SIMULATOR_SCALE; - // qDebug() << "kick at" << p.x() << p.y(); } diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simball.h b/src/extlibs/er_force_sim/src/amun/simulator/simball.h index a7000400a6..1d9022c837 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simball.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simball.h @@ -23,78 +23,83 @@ #include -#include - +#include "extlibs/er_force_sim/src/core/rng.h" #include "extlibs/er_force_sim/src/protobuf/command.pb.h" -#include "extlibs/er_force_sim/src/protobuf/sslsim.h" +#include "extlibs/er_force_sim/src/protobuf/world.pb.h" +#include "proto/ssl_vision_detection.pb.h" #include "shared/constants.h" -#include "simfield.h" - -class RNG; -namespace SSLProto -{ - class SSL_DetectionBall; -} namespace camun { namespace simulator { class SimBall; - enum class ErrorSource; } // namespace simulator } // namespace camun -class camun::simulator::SimBall : public QObject +class camun::simulator::SimBall { - Q_OBJECT public: - SimBall(RNG *rng, btDiscreteDynamicsWorld *world); + SimBall(std::shared_ptr world); ~SimBall(); - SimBall(const SimBall &) = delete; - SimBall &operator=(const SimBall &) = delete; - - signals: - void sendSSLSimError(const SSLSimError &error, ErrorSource s); public: /** - * processes velocity and forces to be applied on the ball - * @param robot_collision whether the ball collides with a robot in this simulation - * tick + * Processes velocity and forces to be applied on the ball + * + * @param robot_collision whether the ball collides with a robot in + * this simulation tick */ void begin(bool robot_collision); - bool update(SSLProto::SSL_DetectionBall *ball, float stddev, float stddevArea, + + bool update(SSLProto::SSL_DetectionBall &ball, float stddev, float stddevArea, const btVector3 &cameraPosition, bool enableInvisibleBall, float visibilityThreshold, btVector3 positionOffset); + void move(const sslsim::TeleportBall &ball); + void kick(const btVector3 &power); - // returns the ball position projected onto the floor (z component is not included) + + /** + * Returns the ball's position projected onto the floor (z component is not included) + * + * @return the ball position + */ btVector3 position() const; + + /** + * Returns the ball's linear velocity + * + * @return the ball velocity + */ btVector3 speed() const; - void writeBallState(world::SimBall *ball) const; + + void writeBallState(world::SimBall &ball) const; + void restoreState(const world::SimBall &ball); + btRigidBody *body() const { - return m_body; + return m_body.get(); } + bool isInvalid() const; // can be used to add ball mis-detections - bool addDetection(SSLProto::SSL_DetectionBall *ball, btVector3 pos, float stddev, + bool addDetection(SSLProto::SSL_DetectionBall &ball, btVector3 pos, float stddev, float stddevArea, const btVector3 &cameraPosition, bool enableInvisibleBall, float visibilityThreshold, btVector3 positionOffset); private: - RNG *m_rng; - btDiscreteDynamicsWorld *m_world; - btCollisionShape *m_sphere; - btRigidBody *m_body; - btMotionState *m_motionState; + RNG m_rng; + std::shared_ptr m_world; + std::unique_ptr m_sphere; + std::unique_ptr m_body; + std::unique_ptr m_motionState; sslsim::TeleportBall m_move; - double rolling_speed; - bool set_transition_speed; + double m_rolling_speed; + bool m_set_transition_speed; enum BallState { @@ -103,7 +108,8 @@ class camun::simulator::SimBall : public QObject SLIDING, ROLLING }; - BallState current_ball_state; + + BallState m_current_ball_state; }; #endif // SIMBALL_H diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp index 78b4e9e510..38acbaff55 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.cpp @@ -24,109 +24,99 @@ using namespace camun::simulator; -SimField::SimField(btDiscreteDynamicsWorld *world, const world::Geometry &geometry) +SimField::SimField(std::shared_ptr world, + const world::Geometry &geometry) : m_world(world) { const float totalWidth = geometry.field_width() / 2.0f + geometry.boundary_width(); const float totalHeight = geometry.field_height() / 2.0f + geometry.boundary_width(); - // upper boundary - const float roomHeight = 8.0f; - const float height = geometry.field_height() / 2.0f - geometry.line_width(); + const float roomHeight = 8.0f; // Upper boundary of "room" that the field lives in + const float height = geometry.field_height() / 2.0f - geometry.line_width(); const float goalWidthHalf = geometry.goal_width() / 2.0f + geometry.goal_wall_width(); const float goalHeightHalf = geometry.goal_height() / 2.0f; const float goalDepth = geometry.goal_depth() + geometry.goal_wall_width(); const float goalDepthHalf = goalDepth / 2.0f; const float goalWallHalf = geometry.goal_wall_width() / 2.0f; - // obstacle prototypes - m_plane = new btStaticPlaneShape(btVector3(0, 0, 1), 0); - m_goalSide = new btBoxShape(btVector3(goalWallHalf, goalDepthHalf, goalHeightHalf) * - SIMULATOR_SCALE); - m_goalBack = new btBoxShape(btVector3(goalWidthHalf, goalWallHalf, goalHeightHalf) * - SIMULATOR_SCALE); + // Collision shapes + m_plane = std::make_unique(btVector3(0, 0, 1), 0); + m_goalSide = std::make_unique( + btVector3(goalWallHalf, goalDepthHalf, goalHeightHalf) * SIMULATOR_SCALE); + m_goalBack = std::make_unique( + btVector3(goalWidthHalf, goalWallHalf, goalHeightHalf) * SIMULATOR_SCALE); - // build field cube - // floor - addObject(m_plane, + // Create the "room" that the field lives in + // Floor + addObject(m_plane.get(), btTransform(btQuaternion(btVector3(1, 0, 0), 0), btVector3(0, 0, 0) * SIMULATOR_SCALE), 0.56, 0.35); - // others - addObject(m_plane, + // Roof + addObject(m_plane.get(), btTransform(btQuaternion(btVector3(1, 0, 0), M_PI), btVector3(0, 0, roomHeight) * SIMULATOR_SCALE), 0.3, 0.35); - - addObject(m_plane, + // Walls + addObject(m_plane.get(), btTransform(btQuaternion(btVector3(1, 0, 0), M_PI_2), btVector3(0, totalHeight, 0) * SIMULATOR_SCALE), 0.3, 0.35); - addObject(m_plane, + addObject(m_plane.get(), btTransform(btQuaternion(btVector3(1, 0, 0), -M_PI_2), btVector3(0, -totalHeight, 0) * SIMULATOR_SCALE), 0.3, 0.35); - - addObject(m_plane, + addObject(m_plane.get(), btTransform(btQuaternion(btVector3(0, 1, 0), M_PI_2), btVector3(-totalWidth, 0, 0) * SIMULATOR_SCALE), 0.3, 0.35); - addObject(m_plane, + addObject(m_plane.get(), btTransform(btQuaternion(btVector3(0, 1, 0), -M_PI_2), btVector3(totalWidth, 0, 0) * SIMULATOR_SCALE), 0.3, 0.35); - // create goals - for (int goal = 0; goal < 2; goal++) + // Create goals + for (const float side : {-1.0f, 1.0f}) { - const float side = (goal == 0) ? -1.0f : 1.0f; - const btQuaternion rot = btQuaternion::getIdentity(); - - addObject( - m_goalSide, - btTransform(rot, btVector3((goalWidthHalf - goalWallHalf), - side * (height + goalDepthHalf), goalHeightHalf) * - SIMULATOR_SCALE), - 0.3, 0.5); - addObject( - m_goalSide, - btTransform(rot, btVector3(-(goalWidthHalf - goalWallHalf), - side * (height + goalDepthHalf), goalHeightHalf) * - SIMULATOR_SCALE), - 0.3, 0.5); - addObject( - m_goalBack, - btTransform(rot, btVector3(0.0f, side * (height + goalDepth - goalWallHalf), - goalHeightHalf) * - SIMULATOR_SCALE), - 0.1, 0.5); + addObject(m_goalSide.get(), + btTransform(btQuaternion::getIdentity(), + btVector3((goalWidthHalf - goalWallHalf), + side * (height + goalDepthHalf), goalHeightHalf) * + SIMULATOR_SCALE), + 0.3, 0.5); + addObject(m_goalSide.get(), + btTransform(btQuaternion::getIdentity(), + btVector3(-(goalWidthHalf - goalWallHalf), + side * (height + goalDepthHalf), goalHeightHalf) * + SIMULATOR_SCALE), + 0.3, 0.5); + addObject(m_goalBack.get(), + btTransform(btQuaternion::getIdentity(), + btVector3(0.0f, side * (height + goalDepth - goalWallHalf), + goalHeightHalf) * + SIMULATOR_SCALE), + 0.1, 0.5); } } SimField::~SimField() { - foreach (btCollisionObject *object, m_objects) + for (const auto &object : m_objects) { - m_world->removeCollisionObject(object); - delete object; + m_world->removeCollisionObject(object.get()); } - - delete m_goalSide; - delete m_goalBack; - delete m_plane; } void SimField::addObject(btCollisionShape *shape, const btTransform &transform, float restitution, float friction) { - // create new obstacle - btCollisionObject *object = new btCollisionObject; + std::unique_ptr object = std::make_unique(); + object->setCollisionShape(shape); - // damp ball a bit if it hits an obstacle + object->setWorldTransform(transform); object->setRestitution(restitution); - // the friction is multiplied with the colliding obstacle ones object->setFriction(friction); object->setRollingFriction(friction); - object->setWorldTransform(transform); - m_world->addCollisionObject(object); - m_objects.append(object); + + m_world->addCollisionObject(object.get()); + m_objects.push_back(std::move(object)); } diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h index 8582be8a21..b7743d2f43 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h @@ -23,8 +23,6 @@ #include -#include - #include "extlibs/er_force_sim/src/protobuf/world.pb.h" namespace camun @@ -32,27 +30,26 @@ namespace camun namespace simulator { class SimField; - } + } // namespace simulator } // namespace camun class camun::simulator::SimField { public: - SimField(btDiscreteDynamicsWorld *world, const world::Geometry &geometry); + SimField(std::shared_ptr world, + const world::Geometry &geometry); ~SimField(); - SimField(const SimField &) = delete; - SimField &operator=(const SimField &) = delete; private: void addObject(btCollisionShape *shape, const btTransform &transform, float restitution, float friction); private: - btDiscreteDynamicsWorld *m_world; - btCollisionShape *m_plane; - btCollisionShape *m_goalSide; - btCollisionShape *m_goalBack; - QList m_objects; + std::shared_ptr m_world; + std::unique_ptr m_plane; + std::unique_ptr m_goalSide; + std::unique_ptr m_goalBack; + std::vector> m_objects; }; #endif // SIMFIELD_H diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp index ddd3767992..571dfd38e9 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.cpp @@ -20,57 +20,51 @@ #include "simrobot.h" +#include #include #include "extlibs/er_force_sim/src/core/coordinates.h" -#include "extlibs/er_force_sim/src/core/rng.h" -#include "mesh.h" -#include "proto/ssl_vision_detection.pb.h" -#include "simball.h" +#include "robotmesh.h" #include "simulator.h" using namespace camun::simulator; const float MAX_SPEED = 1000; -float boundSpeed(float speed) -{ - return qBound(-MAX_SPEED, speed, MAX_SPEED); -} - -SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld *world, - const btVector3 &pos, float dir) - : m_rng(rng), - m_specs(specs), +SimRobot::SimRobot(const robot::Specs &specs, + std::shared_ptr world, const btVector3 &pos, + float dir) + : m_specs(specs), m_world(world), m_charge(false), m_isCharged(false), m_inStandby(false), m_shootTime(0.0), m_commandTime(0.0), - error_sum_v_s(0), - error_sum_v_f(0), - error_sum_omega(0) + m_error_sum_v_s(0), + m_error_sum_v_f(0), + m_error_sum_omega(0) { - btCompoundShape *wholeShape = new btCompoundShape; + std::unique_ptr wholeShape = std::make_unique(); btTransform robotShapeTransform; robotShapeTransform.setIdentity(); // subtract collision margin from dimensions - Mesh mesh(m_specs.radius() - COLLISION_MARGIN / SIMULATOR_SCALE, - m_specs.height() - 2 * COLLISION_MARGIN / SIMULATOR_SCALE, m_specs.angle(), - 0.04f, m_specs.dribbler_height() + 0.02f); - for (const QList &hullPart : mesh.hull()) + auto mesh = + createRobotMesh(m_specs.radius() - COLLISION_MARGIN / SIMULATOR_SCALE, + m_specs.height() - 2 * COLLISION_MARGIN / SIMULATOR_SCALE, + m_specs.angle(), 0.04f, m_specs.dribbler_height() + 0.02f); + for (const auto &hullPart : mesh) { - btConvexHullShape *hullPartShape = new btConvexHullShape; - m_shapes.append(hullPartShape); - for (const QVector3D &v : hullPart) + std::unique_ptr hullPartShape = + std::make_unique(); + for (const auto &[x, y, z] : hullPart) { - hullPartShape->addPoint(btVector3(v.x(), v.y(), v.z()) * SIMULATOR_SCALE); + hullPartShape->addPoint(btVector3(x, y, z) * SIMULATOR_SCALE); } - wholeShape->addChildShape(robotShapeTransform, hullPartShape); + wholeShape->addChildShape(robotShapeTransform, hullPartShape.get()); + m_shapes.push_back(std::move(hullPartShape)); } - m_shapes.append(wholeShape); btTransform startWorldTransform; startWorldTransform.setIdentity(); @@ -78,24 +72,25 @@ SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld SIMULATOR_SCALE); startWorldTransform.setOrigin(robotBasePos); startWorldTransform.setRotation(btQuaternion(btVector3(0, 0, 1), dir - M_PI_2)); - m_motionState = new btDefaultMotionState(startWorldTransform); + m_motionState = std::make_unique(startWorldTransform); // set robot dynamics and move to start position btVector3 localInertia(0, 0, 0); const float robotMassProportion = 49.0f / 50.0f; wholeShape->calculateLocalInertia(robotMassProportion * m_specs.mass(), localInertia); - btRigidBody::btRigidBodyConstructionInfo rbInfo( - robotMassProportion * m_specs.mass(), m_motionState, wholeShape, localInertia); + btRigidBody::btRigidBodyConstructionInfo rbInfo(robotMassProportion * m_specs.mass(), + m_motionState.get(), wholeShape.get(), + localInertia); - m_body = new btRigidBody(rbInfo); + m_body = std::make_unique(rbInfo); // see simulator.cpp m_body->setRestitution(0.6f); m_body->setFriction(0.22f); - m_world->addRigidBody(m_body); + m_world->addRigidBody(m_body.get()); - btCylinderShape *dribblerShape = new btCylinderShapeX( + std::unique_ptr dribblerShape = std::make_unique( btVector3(m_specs.dribbler_width() / 2.0f, 0.007f, 0.007f) * SIMULATOR_SCALE); - m_shapes.append(dribblerShape); + // WARNING: hack, instead of 0.02 should be the dribbler height // the ball seems to get instable if the dribbler is at correct height // possibly the ball gets 'sucked' onto the robot @@ -111,15 +106,14 @@ SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld dribblerShape->calculateLocalInertia((1 - robotMassProportion) * m_specs.mass(), dribblerInertia); btRigidBody::btRigidBodyConstructionInfo rbDribInfo( - (1 - robotMassProportion) * m_specs.mass(), nullptr, dribblerShape, + (1 - robotMassProportion) * m_specs.mass(), nullptr, dribblerShape.get(), dribblerInertia); rbDribInfo.m_startWorldTransform = dribblerStartTransform; - btRigidBody *dribblerBody = new btRigidBody(rbDribInfo); - dribblerBody->setRestitution(0.2f); - dribblerBody->setFriction(1.5f); - m_dribblerBody = dribblerBody; - m_world->addRigidBody(dribblerBody); + m_dribblerBody = std::make_unique(rbDribInfo); + m_dribblerBody->setRestitution(0.2f); + m_dribblerBody->setFriction(1.5f); + m_world->addRigidBody(m_dribblerBody.get()); btTransform localA, localB; localA.setIdentity(); @@ -127,9 +121,13 @@ SimRobot::SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld localA.setOrigin(m_dribblerCenter); localA.setRotation(btQuaternion(btVector3(0, 1, 0), M_PI_2)); localB.setRotation(btQuaternion(btVector3(0, 1, 0), M_PI_2)); - m_dribblerConstraint = new btHingeConstraint(*m_body, *dribblerBody, localA, localB); + m_dribblerConstraint = + std::make_unique(*m_body, *m_dribblerBody, localA, localB); m_dribblerConstraint->enableAngularMotor(false, 0, 0); - m_world->addConstraint(m_dribblerConstraint, true); + m_world->addConstraint(m_dribblerConstraint.get(), true); + + m_shapes.push_back(std::move(wholeShape)); + m_shapes.push_back(std::move(dribblerShape)); } SimRobot::~SimRobot() @@ -138,14 +136,9 @@ SimRobot::~SimRobot() { m_world->removeConstraint(m_holdBallConstraint.get()); } - m_world->removeConstraint(m_dribblerConstraint); - m_world->removeRigidBody(m_dribblerBody); - m_world->removeRigidBody(m_body); - delete m_dribblerConstraint; - delete m_body; - delete m_dribblerBody; - delete m_motionState; - qDeleteAll(m_shapes); + m_world->removeConstraint(m_dribblerConstraint.get()); + m_world->removeRigidBody(m_dribblerBody.get()); + m_world->removeRigidBody(m_body.get()); } void SimRobot::calculateDribblerMove(const btVector3 pos, const btQuaternion rot, @@ -165,7 +158,7 @@ void SimRobot::calculateDribblerMove(const btVector3 pos, const btQuaternion rot m_dribblerBody->setAngularVelocity(btVector3(0, 0, 0)); } -void SimRobot::dribble(SimBall *ball, float speed) +void SimRobot::dribble(const SimBall &ball, float speed) { if (m_perfectDribbler) { @@ -176,12 +169,12 @@ void SimRobot::dribble(SimBall *ball, float speed) localB.setIdentity(); auto worldToRobot = m_body->getWorldTransform().inverse(); - localA.setOrigin(worldToRobot * ball->position()); + localA.setOrigin(worldToRobot * ball.position()); localA.setRotation(btQuaternion(worldToRobot * btVector3(0, 1, 0), M_PI_2)); localB.setRotation(btQuaternion(worldToRobot * btVector3(0, 1, 0), M_PI_2)); - m_holdBallConstraint.reset( - new btHingeConstraint(*m_body, *ball->body(), localA, localB)); + m_holdBallConstraint = std::make_unique( + *m_body, *ball.body(), localA, localB); m_world->addConstraint(m_holdBallConstraint.get(), true); } } @@ -191,7 +184,7 @@ void SimRobot::dribble(SimBall *ball, float speed) const float max_rotation_speed = 150.f / 2 / M_PI * 60; float dribbler = speed / max_rotation_speed; // rad/s is limited to 150 - float boundedDribbler = qBound(0.0f, dribbler, 1.0f); + float boundedDribbler = std::clamp(dribbler, 0.0f, 1.0f); m_dribblerConstraint->enableAngularMotor(true, 150 * boundedDribbler, 20 * boundedDribbler); } @@ -217,7 +210,7 @@ void SimRobot::setDribbleMode(bool perfectDribbler) m_perfectDribbler = perfectDribbler; } -void SimRobot::begin(SimBall *ball, double time) +void SimRobot::begin(SimBall &ball, double time) { m_commandTime += time; m_inStandby = false; @@ -243,20 +236,14 @@ void SimRobot::begin(SimBall *ball, double time) } auto sendPartialCoordError = [this](const std::string &msg) { - SSLSimError error{new sslsim::SimulatorError}; - error->set_code("PARTIAL_COORD"); - std::string message = "Partial coordinates are not implemented yet"; - error->set_message(message + msg); - emit this->sendSSLSimError(error, ErrorSource::CONFIG); + std::cerr << "Partial coordinates are not implemented yet" << msg << std::endl; if (!m_move.has_by_force() || !m_move.by_force()) { m_move.Clear(); } }; bool moveCommand = false; - std::string message = " for robot ("; - message += std::to_string(m_specs.id()); - message += ')'; + std::string message = " for robot (" + std::to_string(m_specs.id()) + ")"; if (m_move.has_x()) { @@ -309,10 +296,7 @@ void SimRobot::begin(SimBall *ball, double time) } if (sendError) { - SSLSimError error{new sslsim::SimulatorError}; - error->set_code("VELOCITY_FORCE"); - error->set_message("Velocities != 0 and by_force are incompatible"); - emit sendSSLSimError(error, ErrorSource::CONFIG); + std::cerr << "Velocities != 0 and by_force are incompatible" << std::endl; return; } } // TODO: check for force and orientation @@ -425,23 +409,23 @@ void SimRobot::begin(SimBall *ball, double time) // we subtract the current speed of the ball from the intended kick speed // this ensures the ball leaves the robot at exactly the speed we want float kickSpeed = 0.0f; - if (ball->speed().length() < kickSpeedBoundThreshold) + if (ball.speed().length() < kickSpeedBoundThreshold) { - kickSpeed = m_sslCommand.kick_speed() - - (ball->speed().length() / SIMULATOR_SCALE); + kickSpeed = + m_sslCommand.kick_speed() - (ball.speed().length() / SIMULATOR_SCALE); } else { kickSpeed = m_sslCommand.kick_speed(); } - power = qBound(0.05f, kickSpeed, m_specs.shot_linear_max()); + power = std::clamp(kickSpeed, 0.05f, m_specs.shot_linear_max()); } else { // FIXME: for now we just recalc the max distance based on the given angle const float maxShootSpeed = coordinates::chipVelFromChipDistance(m_specs.shot_chip_max()); - power = qBound(0.05f, m_sslCommand.kick_speed(), maxShootSpeed); + power = std::clamp(m_sslCommand.kick_speed(), 0.05f, maxShootSpeed); } const auto getSpeedCompensation = [&]() -> float { @@ -453,15 +437,16 @@ void SimRobot::begin(SimBall *ball, double time) { // if the ball hits the robot the chip distance actually decreases const btVector3 relBallSpeed = relativeBallSpeed(ball) / SIMULATOR_SCALE; - return std::max((btScalar)0, relBallSpeed.y()) - - qBound((btScalar)0, (btScalar)0.5 * relBallSpeed.y(), - (btScalar)0.5 * dirFloor); + return std::max(static_cast(0), relBallSpeed.y()) - + std::clamp(static_cast(0.5) * relBallSpeed.y(), + static_cast(0), + static_cast(0.5) * dirFloor); } }; const float speedCompensation = getSpeedCompensation(); - ball->kick(t * btVector3(0, dirFloor * power + speedCompensation, dirUp * power) * - (1.0f / static_cast(time)) * SIMULATOR_SCALE * - static_cast(BALL_MASS_KG)); + ball.kick(t * btVector3(0, dirFloor * power + speedCompensation, dirUp * power) * + (1.0f / static_cast(time)) * SIMULATOR_SCALE * + static_cast(BALL_MASS_KG)); // discharge m_isCharged = false; m_shootTime = 0.0; @@ -478,7 +463,8 @@ void SimRobot::begin(SimBall *ball, double time) float output_omega = m_sslCommand.move_command().local_velocity().angular(); btVector3 v_local(t.inverse() * m_body->getLinearVelocity()); - btVector3 v_d_local(boundSpeed(output_v_s), boundSpeed(output_v_f), 0); + btVector3 v_d_local(std::clamp(output_v_s, -MAX_SPEED, MAX_SPEED), + std::clamp(output_v_f, -MAX_SPEED, MAX_SPEED), 0); float v_f = v_local.y() / SIMULATOR_SCALE; float v_s = v_local.x() / SIMULATOR_SCALE; @@ -486,15 +472,15 @@ void SimRobot::begin(SimBall *ball, double time) const float error_v_s = v_d_local.x() - v_s; const float error_v_f = v_d_local.y() - v_f; - const float error_omega = boundSpeed(output_omega) - omega; + const float error_omega = std::clamp(output_omega, -MAX_SPEED, MAX_SPEED) - omega; - error_sum_v_s += error_v_s; - error_sum_v_f += error_v_f; - error_sum_omega += error_omega; + m_error_sum_v_s += error_v_s; + m_error_sum_v_f += error_v_f; + m_error_sum_omega += error_omega; - const float error_sum_limit = 20.0f; - error_sum_v_s = qBound(-error_sum_limit, error_sum_v_s, error_sum_limit); - error_sum_v_f = qBound(-error_sum_limit, error_sum_v_f, error_sum_limit); + const float m_error_sum_limit = 20.0f; + m_error_sum_v_s = std::clamp(m_error_sum_v_s, -m_error_sum_limit, m_error_sum_limit); + m_error_sum_v_f = std::clamp(m_error_sum_v_f, -m_error_sum_limit, m_error_sum_limit); // (1-(1-linear_damping)^timestep)/timestep - compensates damping const float V = 1.200f; // keep current speed @@ -504,8 +490,8 @@ void SimRobot::begin(SimBall *ball, double time) // as a certain part of the acceleration is required to compensate damping, // the robot will run into a speed limit! bound acceleration the speed limit // is acceleration * accelScale / V - float a_f = V * v_f + K * error_v_f + K_I * error_sum_v_f; - float a_s = V * v_s + K * error_v_s + K_I * error_sum_v_s; + float a_f = V * v_f + K * error_v_f + K_I * m_error_sum_v_f; + float a_s = V * v_s + K * error_v_s + K_I * m_error_sum_v_s; const float accelScale = 2.f; // let robot accelerate / brake faster than the accelerator does @@ -524,7 +510,7 @@ void SimRobot::begin(SimBall *ball, double time) 1.f / time * 0.5f; // correct half the error during each subtimestep const float K_I_phi = /*0*0.2/1000; //*/ 0.f; - const float a_phi = V_phi * omega + K_phi * error_omega + K_I_phi * error_sum_omega; + const float a_phi = V_phi * omega + K_phi * error_omega + K_I_phi * m_error_sum_omega; const float a_phi_bound = bound(a_phi, omega, accelScale * m_specs.strategy().a_speedup_phi_max(), accelScale * m_specs.strategy().a_brake_phi_max()); @@ -546,19 +532,19 @@ float SimRobot::bound(float acceleration, float oldSpeed, float speedupLimit, if ((std::signbit(acceleration) == std::signbit(oldSpeed)) || (oldSpeed == 0)) { // the acceleration needs to be bounded with values for speeding up. - return qBound(-speedupLimit, acceleration, speedupLimit); + return std::clamp(acceleration, -speedupLimit, speedupLimit); } else { // bound braking acceleration, in order to avoid fallover - return qBound(-brakeLimit, acceleration, brakeLimit); + return std::clamp(acceleration, -brakeLimit, brakeLimit); } } -btVector3 SimRobot::relativeBallSpeed(SimBall *ball) const +btVector3 SimRobot::relativeBallSpeed(const SimBall &ball) const { btTransform t = m_body->getWorldTransform(); - const btVector3 ballSpeed = ball->speed(); + const btVector3 ballSpeed = ball.speed(); const btQuaternion robotDir = t.getRotation(); const btVector3 diff = (ballSpeed).rotate(robotDir.getAxis(), -robotDir.getAngle()); @@ -566,9 +552,9 @@ btVector3 SimRobot::relativeBallSpeed(SimBall *ball) const return diff; } -bool SimRobot::canKickBall(SimBall *ball) const +bool SimRobot::canKickBall(const SimBall &ball) const { - const btVector3 ballPos = ball->position(); + const btVector3 ballPos = ball.position(); // can't kick jumping ball if (ballPos.z() > 0.05f * SIMULATOR_SCALE) { @@ -588,8 +574,8 @@ bool SimRobot::canKickBall(SimBall *ball) const m_world->getDispatcher()->getManifoldByIndexInternal(i); btCollisionObject *objectA = (btCollisionObject *)(contactManifold->getBody0()); btCollisionObject *objectB = (btCollisionObject *)(contactManifold->getBody1()); - if ((objectA == m_dribblerBody && objectB == ball->body()) || - (objectA == ball->body() && objectB == m_dribblerBody)) + if ((objectA == m_dribblerBody.get() && objectB == ball.body()) || + (objectA == ball.body() && objectB == m_dribblerBody.get())) { int numContacts = contactManifold->getNumContacts(); for (int j = 0; j < numContacts; ++j) @@ -606,7 +592,7 @@ bool SimRobot::canKickBall(SimBall *ball) const } robot::RadioResponse SimRobot::setCommand(const SSLSimulationProto::RobotCommand &command, - SimBall *ball, bool charge, float rxLoss, + const SimBall &ball, bool charge, float rxLoss, float txLoss) { m_sslCommand = command; @@ -639,31 +625,31 @@ robot::RadioResponse SimRobot::setCommand(const SSLSimulationProto::RobotCommand return response; } -void SimRobot::update(SSLProto::SSL_DetectionRobot *robot, float stddev_p, - float stddev_phi, qint64 time, btVector3 positionOffset) +void SimRobot::update(SSLProto::SSL_DetectionRobot &robot, float stddev_p, + float stddev_phi, int64_t time, btVector3 positionOffset) { // setup vision packet - robot->set_robot_id(m_specs.id()); - robot->set_confidence(1.0); - robot->set_pixel_x(0); - robot->set_pixel_y(0); + robot.set_robot_id(m_specs.id()); + robot.set_confidence(1.0); + robot.set_pixel_x(0); + robot.set_pixel_y(0); // add noise btTransform transform; m_motionState->getWorldTransform(transform); const btVector3 p = transform.getOrigin() / SIMULATOR_SCALE + positionOffset; - const ErForceVector p_noise = m_rng->normalVector(stddev_p); - robot->set_x((p.y() + p_noise.x) * 1000.0f); - robot->set_y(-(p.x() + p_noise.y) * 1000.0f); + const ErForceVector p_noise = m_rng.normalVector(stddev_p); + robot.set_x((p.y() + p_noise.x) * 1000.0f); + robot.set_y(-(p.x() + p_noise.y) * 1000.0f); const btQuaternion q = transform.getRotation(); const btVector3 dir = btMatrix3x3(q).getColumn(0); - robot->set_orientation(atan2(dir.y(), dir.x()) + m_rng->normal(stddev_phi)); + robot.set_orientation(atan2(dir.y(), dir.x()) + m_rng.normal(stddev_phi)); m_lastSendTime = time; } -bool SimRobot::touchesBall(SimBall *ball) const +bool SimRobot::touchesBall(const SimBall &ball) const { // for some reason btHingeConstraints, which is used when dribbling, are not always // detected as contact by bullet. so if the ball is being dribbled then we assume it @@ -684,10 +670,10 @@ bool SimRobot::touchesBall(SimBall *ball) const // determine if the two objects are the ball and robot body/dribbler btCollisionObject *objectA = (btCollisionObject *)(contact_manifold->getBody0()); btCollisionObject *objectB = (btCollisionObject *)(contact_manifold->getBody1()); - if ((objectA == m_dribblerBody && objectB == ball->body()) || - (objectA == ball->body() && objectB == m_dribblerBody) || - (objectA == m_body && objectB == ball->body()) || - (objectA == ball->body() && objectB == m_body)) + if ((objectA == m_dribblerBody.get() && objectB == ball.body()) || + (objectA == ball.body() && objectB == m_dribblerBody.get()) || + (objectA == m_body.get() && objectB == ball.body()) || + (objectA == ball.body() && objectB == m_body.get())) { // check if the points are in contact now int num_contacts = contact_manifold->getNumContacts(); @@ -705,18 +691,18 @@ bool SimRobot::touchesBall(SimBall *ball) const return false; } -void SimRobot::update(world::SimRobot *robot, SimBall *ball) const +void SimRobot::update(world::SimRobot &robot, const SimBall &ball) const { btTransform transform; m_motionState->getWorldTransform(transform); const btVector3 position = transform.getOrigin() / SIMULATOR_SCALE; - robot->set_p_x(position.x()); - robot->set_p_y(position.y()); - robot->set_p_z(position.z()); - robot->set_id(m_specs.id()); + robot.set_p_x(position.x()); + robot.set_p_y(position.y()); + robot.set_p_z(position.z()); + robot.set_id(m_specs.id()); const btQuaternion q = transform.getRotation(); - auto *rotation = robot->mutable_rotation(); + auto *rotation = robot.mutable_rotation(); rotation->set_real(q.getX()); rotation->set_i(q.getY()); rotation->set_j(q.getZ()); @@ -727,17 +713,17 @@ void SimRobot::update(world::SimRobot *robot, SimBall *ball) const float y = 0; float z = 0; q.getEulerZYX(z, y, x); - robot->set_angle(z); + robot.set_angle(z); const btVector3 velocity = m_body->getLinearVelocity() / SIMULATOR_SCALE; - robot->set_v_x(velocity.x()); - robot->set_v_y(velocity.y()); - robot->set_v_z(velocity.z()); + robot.set_v_x(velocity.x()); + robot.set_v_y(velocity.y()); + robot.set_v_z(velocity.z()); const btVector3 angular = m_body->getAngularVelocity(); - robot->set_r_x(angular.x()); - robot->set_r_y(angular.y()); - robot->set_r_z(angular.z()); + robot.set_r_x(angular.x()); + robot.set_r_y(angular.y()); + robot.set_r_z(angular.z()); bool ballTouchesRobot = false; int numManifolds = m_world->getDispatcher()->getNumManifolds(); @@ -747,15 +733,15 @@ void SimRobot::update(world::SimRobot *robot, SimBall *ball) const m_world->getDispatcher()->getManifoldByIndexInternal(i); btCollisionObject *objectA = (btCollisionObject *)(contactManifold->getBody0()); btCollisionObject *objectB = (btCollisionObject *)(contactManifold->getBody1()); - if ((objectA == m_dribblerBody && objectB == ball->body()) || - (objectA == ball->body() && objectB == m_dribblerBody) || - (objectA == m_body && objectB == ball->body()) || - (objectA == ball->body() && objectB == m_body)) + if ((objectA == m_dribblerBody.get() && objectB == ball.body()) || + (objectA == ball.body() && objectB == m_dribblerBody.get()) || + (objectA == m_body.get() && objectB == ball.body()) || + (objectA == ball.body() && objectB == m_body.get())) { ballTouchesRobot = true; } } - robot->set_touches_ball(ballTouchesRobot); + robot.set_touches_ball(ballTouchesRobot); } void SimRobot::restoreState(const world::SimRobot &robot) diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h index 89c60002cb..c156f80ee5 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h @@ -23,62 +23,62 @@ #include -#include - +#include "extlibs/er_force_sim/src/core/rng.h" #include "extlibs/er_force_sim/src/protobuf/command.pb.h" #include "extlibs/er_force_sim/src/protobuf/robot.pb.h" -#include "extlibs/er_force_sim/src/protobuf/sslsim.h" +#include "extlibs/er_force_sim/src/protobuf/world.pb.h" #include "proto/ssl_simulation_robot_control.pb.h" - -class RNG; -namespace SSLProto -{ - class SSL_DetectionRobot; -} +#include "proto/ssl_vision_detection.pb.h" +#include "simball.h" namespace camun { namespace simulator { - class SimBall; class SimRobot; - enum class ErrorSource; } // namespace simulator } // namespace camun -class camun::simulator::SimRobot : public QObject +class camun::simulator::SimRobot { - Q_OBJECT public: - SimRobot(RNG *rng, const robot::Specs &specs, btDiscreteDynamicsWorld *world, + SimRobot(const robot::Specs &specs, std::shared_ptr world, const btVector3 &pos, float dir); ~SimRobot(); - SimRobot(const SimRobot &) = delete; - SimRobot &operator=(const SimRobot &) = delete; - - signals: - void sendSSLSimError(const SSLSimError &error, ErrorSource s); public: - void begin(SimBall *ball, double time); - bool canKickBall(SimBall *ball) const; - void tryKick(SimBall *ball, float power, double time); + void begin(SimBall &ball, double time); + + bool canKickBall(const SimBall &ball) const; + + void tryKick(const SimBall &ball, float power, double time); + robot::RadioResponse setCommand(const SSLSimulationProto::RobotCommand &command, - SimBall *ball, bool charge, float rxLoss, + const SimBall &ball, bool charge, float rxLoss, float txLoss); - void update(SSLProto::SSL_DetectionRobot *robot, float stddev_p, float stddev_phi, - qint64 time, btVector3 positionOffset); - void update(world::SimRobot *robot, SimBall *ball) const; + + void update(SSLProto::SSL_DetectionRobot &robot, float stddev_p, float stddev_phi, + int64_t time, btVector3 positionOffset); + + void update(world::SimRobot &robot, const SimBall &ball) const; + void restoreState(const world::SimRobot &robot); + void move(const sslsim::TeleportRobot &robot); + bool isFlipped(); + btVector3 position() const; + btVector3 dribblerCorner(bool left) const; - qint64 getLastSendTime() const + + int64_t getLastSendTime() const { return m_lastSendTime; } + void setDribbleMode(bool perfectDribbler); + void stopDribbling(); const robot::Specs &specs() const @@ -92,34 +92,27 @@ class camun::simulator::SimRobot : public QObject * @param ball the ball in play * @return true if the ball is in contact with the robot, false otherwise */ - bool touchesBall(SimBall *ball) const; + bool touchesBall(const SimBall &ball) const; private: - btVector3 relativeBallSpeed(SimBall *ball) const; + btVector3 relativeBallSpeed(const SimBall &ball) const; float bound(float acceleration, float oldSpeed, float speedupLimit, float brakeLimit) const; void calculateDribblerMove(const btVector3 pos, const btQuaternion rot, const btVector3 linVel, float omega); - void dribble(SimBall *ball, float speed); + void dribble(const SimBall &ball, float speed); - RNG *m_rng; + RNG m_rng; robot::Specs m_specs; - btDiscreteDynamicsWorld *m_world; - btRigidBody *m_body; - btRigidBody *m_dribblerBody; - btHingeConstraint *m_dribblerConstraint; - QList m_shapes; - btMotionState *m_motionState; - btVector3 m_dribblerCenter; + std::shared_ptr m_world; + std::unique_ptr m_body; + std::unique_ptr m_dribblerBody; + std::unique_ptr m_dribblerConstraint; + std::vector> m_shapes; + std::unique_ptr m_motionState; std::unique_ptr m_holdBallConstraint; - - struct Wheel - { - float angle; - btVector3 pos; - btVector3 dir; - }; + btVector3 m_dribblerCenter; sslsim::TeleportRobot m_move; SSLSimulationProto::RobotCommand m_sslCommand; @@ -129,13 +122,13 @@ class camun::simulator::SimRobot : public QObject double m_shootTime; double m_commandTime; - float error_sum_v_s; - float error_sum_v_f; - float error_sum_omega; + float m_error_sum_v_s; + float m_error_sum_v_f; + float m_error_sum_omega; bool m_perfectDribbler = false; - qint64 m_lastSendTime = 0; + int64_t m_lastSendTime = 0; }; #endif // SIMROBOT_H diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp index 2058c8a3ae..a37333174e 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp +++ b/src/extlibs/er_force_sim/src/amun/simulator/simulator.cpp @@ -20,135 +20,52 @@ #include "simulator.h" -#include -#include -#include -#include #include #include -#include "erroraggregator.h" #include "extlibs/er_force_sim/src/core/coordinates.h" -#include "extlibs/er_force_sim/src/core/rng.h" #include "extlibs/er_force_sim/src/protobuf/geometry.h" -#include "simball.h" -#include "simfield.h" -#include "simrobot.h" - using namespace camun::simulator; -/* Friction and restitution between robots, ball and field: (empirical - * measurments) Ball vs. Robot: Restitution: about 0.60 Friction: trial and - * error in simulator 0.18 (similar results as in reality) - * - * Ball vs. Floor: - * Restitution: sqrt(h'/h) = sqrt(0.314) = 0.56 - * Friction: \mu_k = -a / g (while slipping) = 0.35 - * - * Robot vs. Floor: - * Restitution and Friction should be as low as possible - * - * Calculations: - * Variables: r: restitution, f: friction - * Indices: b: ball; f: floor; r: robot - * - * r_b * r_f = 0.56 - * r_b * r_r = 0.60 - * r_f * r_r = small - * => r_b = 1; r_f = 0.56; r_r = 0.60 - * - * f_b * f_f = 0.35 - * f_b * f_r = 0.22 - * f_f * f_r = very small - * => f_b = 1; f_f = 0.35; f_r = 0.22 - */ - -struct camun::simulator::SimulatorData -{ - RNG rng; - btDefaultCollisionConfiguration *collision; - btCollisionDispatcher *dispatcher; - btBroadphaseInterface *overlappingPairCache; - btSequentialImpulseConstraintSolver *solver; - btDiscreteDynamicsWorld *dynamicsWorld; - world::Geometry geometry; - QVector reportedCameraSetup; - QVector cameraPositions; - SimField *field; - SimBall *ball; - Simulator::RobotMap robotsBlue; - Simulator::RobotMap robotsYellow; - QMap specsBlue; - QMap specsYellow; - bool flip; - float stddevBall; - float stddevBallArea; - float stddevRobot; - float stddevRobotPhi; - float ballDetectionsAtDribbler; // per robot per second - bool enableInvisibleBall; - float ballVisibilityThreshold; - float cameraOverlap; - float cameraPositionError; - float objectPositionOffset; - float robotCommandPacketLoss; - float robotReplyPacketLoss; - float missingBallDetections; - bool dribblePerfect; -}; - -static void simulatorTickCallback(btDynamicsWorld *world, btScalar timeStep) -{ - Simulator *sim = reinterpret_cast(world->getWorldUserInfo()); - sim->handleSimulatorTick(timeStep); -} - -/*! - * \class Simulator - * \ingroup simulator - * \brief %Simulator interface - */ - Simulator::Simulator(const amun::SimulatorSetup &setup) - : m_time(0), - m_lastSentStatusTime(0), + : m_data(std::make_unique()), m_enabled(false), m_charge(true), + m_time(0), m_visionDelay(35 * 1000 * 1000), - m_visionProcessingTime(5 * 1000 * 1000), - m_aggregator(new ErrorAggregator(this)) + m_visionProcessingTime(5 * 1000 * 1000) { - // setup bullet - m_data = new SimulatorData; - m_data->collision = new btDefaultCollisionConfiguration(); - m_data->dispatcher = new btCollisionDispatcher(m_data->collision); - m_data->overlappingPairCache = new btDbvtBroadphase(); - m_data->solver = new btSequentialImpulseConstraintSolver; - m_data->dynamicsWorld = - new btDiscreteDynamicsWorld(m_data->dispatcher, m_data->overlappingPairCache, - m_data->solver, m_data->collision); + m_data->collision = std::make_unique(); + m_data->dispatcher = std::make_unique(m_data->collision.get()); + m_data->overlappingPairCache = std::make_unique(); + m_data->solver = std::make_unique(); + m_data->dynamicsWorld = std::make_shared( + m_data->dispatcher.get(), m_data->overlappingPairCache.get(), + m_data->solver.get(), m_data->collision.get()); m_data->dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, -9.81f * SIMULATOR_SCALE)); - m_data->dynamicsWorld->setInternalTickCallback(simulatorTickCallback, this, true); + m_data->dynamicsWorld->setInternalTickCallback( + [](btDynamicsWorld *world, btScalar timeStep) { + Simulator *sim = reinterpret_cast(world->getWorldUserInfo()); + sim->handleSimulatorTick(timeStep); + }, + this, true); m_data->geometry.CopyFrom(setup.geometry()); for (const auto &camera : setup.camera_setup()) { - m_data->reportedCameraSetup.append(camera); + m_data->reportedCameraSetup.push_back(camera); ErForceVector visionPosition(camera.derived_camera_world_tx(), camera.derived_camera_world_ty()); btVector3 truePosition; coordinates::fromVision(visionPosition, truePosition); truePosition.setZ(camera.derived_camera_world_tz() / 1000.0f); - m_data->cameraPositions.append(truePosition); + m_data->cameraPositions.push_back(truePosition); } - // add field and ball - m_data->field = new SimField(m_data->dynamicsWorld, m_data->geometry); - m_data->ball = new SimBall(&m_data->rng, m_data->dynamicsWorld); - connect(m_data->ball, &SimBall::sendSSLSimError, m_aggregator, - &ErrorAggregator::aggregate); - m_data->flip = false; + m_data->field = std::make_unique(m_data->dynamicsWorld, m_data->geometry); + m_data->ball = std::make_shared(m_data->dynamicsWorld); + m_data->flip = false; m_data->stddevBall = 0.0f; m_data->stddevBallArea = 0.0f; m_data->stddevRobot = 0.0f; @@ -167,32 +84,6 @@ Simulator::Simulator(const amun::SimulatorSetup &setup) // no robots after initialisation } -// does delete all Simrobots in the RobotMap, does not clear map -// (just like qDeleteAll would) -static void deleteAll(const Simulator::RobotMap &map) -{ - for (const auto &e : map) - { - delete e.first; - } -} - -Simulator::~Simulator() -{ - resetVisionPackets(); - - deleteAll(m_data->robotsBlue); - deleteAll(m_data->robotsYellow); - delete m_data->ball; - delete m_data->field; - delete m_data->dynamicsWorld; - delete m_data->solver; - delete m_data->overlappingPairCache; - delete m_data->dispatcher; - delete m_data->collision; - delete m_data; -} - std::vector Simulator::acceptBlueRobotControlCommand( const SSLSimulationProto::RobotControl &control) { @@ -211,65 +102,35 @@ std::vector Simulator::acceptRobotControlCommand( // collect responses from robots std::vector responses; + Simulator::RobotMap &robotMap = isBlue ? m_data->robotsBlue : m_data->robotsYellow; + for (const SSLSimulationProto::RobotCommand &command : control.robot_commands()) { - // pass radio command to robot that matches the id - const auto id = command.id(); - SimulatorData *data = m_data; - auto time = m_time; - auto charge = m_charge; - auto fabricateResponse = [data, &responses, time, charge, &id, &command]( - const Simulator::RobotMap &map, const bool *isBlue) { - if (!map.contains(id)) - return; - robot::RadioResponse response = map[id].first->setCommand( - command, data->ball, charge, data->robotCommandPacketLoss, - data->robotReplyPacketLoss); - response.set_time(time); - - if (isBlue != nullptr) - { - response.set_is_blue(*isBlue); - } - // only collect valid responses - if (response.IsInitialized()) - { - if (data->robotReplyPacketLoss == 0 || - data->rng.uniformFloat(0, 1) > data->robotReplyPacketLoss) - { - responses.emplace_back(response); - } - } - }; - if (isBlue) + if (!robotMap.contains(command.id())) { - fabricateResponse(m_data->robotsBlue, &isBlue); + return responses; } - else + + // pass radio command to robot that matches the id + robot::RadioResponse response = robotMap.at(command.id()) + ->setCommand(command, *m_data->ball, m_charge, + m_data->robotCommandPacketLoss, + m_data->robotReplyPacketLoss); + response.set_time(m_time); + response.set_is_blue(isBlue); + + // only collect valid responses + if (response.IsInitialized()) { - fabricateResponse(m_data->robotsYellow, &isBlue); + if (m_data->robotReplyPacketLoss == 0 || + m_data->rng.uniformFloat(0, 1) > m_data->robotReplyPacketLoss) + { + responses.emplace_back(response); + } } } - return responses; -} -void Simulator::sendSSLSimErrorInternal(ErrorSource source) -{ - QList errors = m_aggregator->getAggregates(source); - if (errors.size() == 0) - return; - emit sendSSLSimError(errors, source); -} - -static void createRobot(Simulator::RobotMap &list, float x, float y, uint32_t id, - const ErrorAggregator *agg, SimulatorData *data, - const QMap &teamSpecs) -{ - SimRobot *robot = new SimRobot(&data->rng, teamSpecs[id], data->dynamicsWorld, - btVector3(x, y, 0), 0.f); - robot->setDribbleMode(data->dribblePerfect); - robot->connect(robot, &SimRobot::sendSSLSimError, agg, &ErrorAggregator::aggregate); - list[id] = {robot, teamSpecs[id].generation()}; + return responses; } void Simulator::resetFlipped(Simulator::RobotMap &robots, float side) @@ -278,22 +139,13 @@ void Simulator::resetFlipped(Simulator::RobotMap &robots, float side) const float x = m_data->geometry.field_width() / 2 - 0.2; float y = m_data->geometry.field_height() / 2 - 0.2; - for (RobotMap::iterator it = robots.begin(); it != robots.end(); ++it) + for (auto &[robotId, robot] : robots) { - SimRobot *robot = it.value().first; if (robot->isFlipped()) { - SimRobot *new_robot = - new SimRobot(&m_data->rng, robot->specs(), m_data->dynamicsWorld, - btVector3(x, side * y, 0), 0.0f); - delete robot; - connect(new_robot, &SimRobot::sendSSLSimError, m_aggregator, - &ErrorAggregator::aggregate); // TODO? use createRobot instead of - // this. However, doing so naively - // will break the iteration, so I - // left it for now. - new_robot->setDribbleMode(m_data->dribblePerfect); - it.value().first = new_robot; + robot = std::make_unique(robot->specs(), m_data->dynamicsWorld, + btVector3(x, side * y, 0), 0.0f); + robot->setDribbleMode(m_data->dribblePerfect); } y -= 0.3; } @@ -314,17 +166,14 @@ void Simulator::handleSimulatorTick(double time_s) resetFlipped(m_data->robotsYellow, -1.0f); if (m_data->ball->isInvalid()) { - delete m_data->ball; - m_data->ball = new SimBall(&m_data->rng, m_data->dynamicsWorld); - connect(m_data->ball, &SimBall::sendSSLSimError, m_aggregator, - &ErrorAggregator::aggregate); + m_data->ball = std::make_shared(m_data->dynamicsWorld); } // find out if ball and any robot collide - auto robot_ball_collision = [this](QPair elem) { - return elem.first->touchesBall(this->m_data->ball); + auto robot_ball_collision = [&](const auto &kv_pair) { + auto &[robotId, robot] = kv_pair; + return robot->touchesBall(*m_data->ball); }; - bool ball_collision = std::any_of(m_data->robotsBlue.begin(), m_data->robotsBlue.end(), robot_ball_collision) || std::any_of(m_data->robotsYellow.begin(), @@ -332,13 +181,13 @@ void Simulator::handleSimulatorTick(double time_s) // apply commands and forces to ball and robots m_data->ball->begin(ball_collision); - for (const auto &pair : m_data->robotsBlue) + for (auto &[robotId, robot] : m_data->robotsBlue) { - pair.first->begin(m_data->ball, time_s); + robot->begin(*m_data->ball, time_s); } - for (const auto &pair : m_data->robotsYellow) + for (auto &[robotId, robot] : m_data->robotsYellow) { - pair.first->begin(m_data->ball, time_s); + robot->begin(*m_data->ball, time_s); } // add gravity to all ACTIVE objects @@ -347,7 +196,8 @@ void Simulator::handleSimulatorTick(double time_s) } static bool checkCameraID(const int cameraId, const btVector3 &p, - const QVector &cameraPositions, const float overlap) + const std::vector &cameraPositions, + const float overlap) { float minDistance = std::numeric_limits::max(); float ownDistance = 0; @@ -366,13 +216,13 @@ static bool checkCameraID(const int cameraId, const btVector3 &p, return ownDistance <= minDistance + 2 * overlap; } -void Simulator::initializeDetection(SSLProto::SSL_DetectionFrame *detection, - std::size_t cameraId) +void Simulator::initializeDetection(SSLProto::SSL_DetectionFrame &detection, + size_t cameraId) { - detection->set_frame_number(m_lastFrameNumber[cameraId]++); - detection->set_camera_id(cameraId); - detection->set_t_capture((m_time + m_visionDelay - m_visionProcessingTime) * 1E-9); - detection->set_t_sent((m_time + m_visionDelay) * 1E-9); + detection.set_frame_number(m_lastFrameNumber[cameraId]++); + detection.set_camera_id(cameraId); + detection.set_t_capture((m_time + m_visionDelay - m_visionProcessingTime) * 1E-9); + detection.set_t_sent((m_time + m_visionDelay) * 1E-9); } static btVector3 positionOffsetForCamera(float offsetStrength, btVector3 cameraPos) @@ -398,7 +248,7 @@ std::vector Simulator::getWrapperPackets() std::vector detections(numCameras); for (std::size_t i = 0; i < numCameras; i++) { - initializeDetection(&detections[i], i); + initializeDetection(detections[i], i); } bool missingBall = m_data->missingBallDetections > 0 && @@ -421,7 +271,7 @@ std::vector Simulator::getWrapperPackets() const btVector3 positionOffset = positionOffsetForCamera( m_data->objectPositionOffset, m_data->cameraPositions[cameraId]); bool visible = m_data->ball->update( - detections[cameraId].add_balls(), m_data->stddevBall, + *detections[cameraId].add_balls(), m_data->stddevBall, m_data->stddevBallArea, m_data->cameraPositions[cameraId], m_data->enableInvisibleBall, m_data->ballVisibilityThreshold, positionOffset); @@ -433,14 +283,12 @@ std::vector Simulator::getWrapperPackets() } // get robot positions - for (bool teamIsBlue : {true, false}) + for (auto &[teamIsBlue, team] : + {std::make_tuple(true, std::ref(m_data->robotsBlue)), + std::make_tuple(false, std::ref(m_data->robotsYellow))}) { - auto &team = teamIsBlue ? m_data->robotsBlue : m_data->robotsYellow; - - for (const auto &it : team) + for (auto &[robotId, robot] : team) { - SimRobot *robot = it.first; - if (m_time - robot->getLastSendTime() >= m_minRobotDetectionTime) { const float timeDiff = (m_time - robot->getLastSendTime()) * 1E-9; @@ -458,13 +306,13 @@ std::vector Simulator::getWrapperPackets() m_data->objectPositionOffset, m_data->cameraPositions[cameraId]); if (teamIsBlue) { - robot->update(detections[cameraId].add_robots_blue(), + robot->update(*detections[cameraId].add_robots_blue(), m_data->stddevRobot, m_data->stddevRobotPhi, m_time, positionOffset); } else { - robot->update(detections[cameraId].add_robots_yellow(), + robot->update(*detections[cameraId].add_robots_yellow(), m_data->stddevRobot, m_data->stddevRobotPhi, m_time, positionOffset); } @@ -478,7 +326,7 @@ std::vector Simulator::getWrapperPackets() { // always on the right side of the dribbler for now if (!m_data->ball->addDetection( - detections[cameraId].add_balls(), + *detections[cameraId].add_balls(), robot->dribblerCorner(false) / SIMULATOR_SCALE, m_data->stddevRobot, 0, m_data->cameraPositions[cameraId], false, 0, positionOffset)) @@ -555,70 +403,46 @@ world::SimulatorState Simulator::getSimulatorState() const std::size_t numCameras = m_data->reportedCameraSetup.size(); world::SimulatorState simState; - auto *ball = simState.mutable_ball(); - m_data->ball->writeBallState(ball); + m_data->ball->writeBallState(*simState.mutable_ball()); // get robot positions for (bool teamIsBlue : {true, false}) { auto &team = teamIsBlue ? m_data->robotsBlue : m_data->robotsYellow; - for (const auto &it : team) + for (auto &[robotId, robot] : team) { - SimRobot *robot = it.first; - // convert coordinates from ER Force btVector3 robotPos = robot->position() / SIMULATOR_SCALE; btVector3 newRobotPos; coordinates::toVision(robotPos, newRobotPos); - auto *robotProto = - teamIsBlue ? simState.add_blue_robots() : simState.add_yellow_robots(); + auto &robotProto = + teamIsBlue ? *simState.add_blue_robots() : *simState.add_yellow_robots(); - robot->update(robotProto, m_data->ball); + robot->update(robotProto, *m_data->ball); // Convert mm to m - robotProto->set_p_x(newRobotPos.x() / 1000); - robotProto->set_p_y(newRobotPos.y() / 1000); + robotProto.set_p_x(newRobotPos.x() / 1000); + robotProto.set_p_y(newRobotPos.y() / 1000); // Convert velocity - coordinates::toVisionVelocity(*robotProto, *robotProto); - robotProto->set_v_x(robotProto->v_x() / 1000); - robotProto->set_v_y(robotProto->v_y() / 1000); + coordinates::toVisionVelocity(robotProto, robotProto); + robotProto.set_v_x(robotProto.v_x() / 1000); + robotProto.set_v_y(robotProto.v_y() / 1000); } } return simState; } -void Simulator::resetVisionPackets() -{ - qDeleteAll(m_visionTimers); - m_visionTimers.clear(); - m_visionPackets.clear(); -} - -void Simulator::handleRadioCommands(const SSLSimRobotControl &commands, bool isBlue, - qint64 processingStart) -{ - m_radioCommands.enqueue(std::make_tuple(commands, processingStart, isBlue)); -} - -void Simulator::setTeam(Simulator::RobotMap &list, float side, const robot::Team &team, - QMap &teamSpecs) +void Simulator::setTeam(Simulator::RobotMap &robotMap, float side, + const robot::Team &team, + std::map &teamSpecs) { // remove old team - deleteAll(list); - list.clear(); - - // changing a team is also triggering a tracking reset - // thus the old robots will disappear immediatelly - // however if the delayed vision packets arrive the old robots will be tracked - // again thus after removing a robot from a team it can take 1 simulated - // second for the robot to disappear to prevent this remove outdated vision - // packets - resetVisionPackets(); + robotMap.clear(); // align robots on a line const float x = m_data->geometry.field_width() / 2 - 0.2; @@ -630,14 +454,17 @@ void Simulator::setTeam(Simulator::RobotMap &list, float side, const robot::Team const auto id = specs.id(); // (color, robot id) must be unique - if (list.contains(id)) + if (robotMap.contains(id)) { std::cerr << "Error: Two ids for the same color, aborting!" << std::endl; continue; } teamSpecs[id].CopyFrom(specs); - createRobot(list, x, side * y, id, m_aggregator, m_data, teamSpecs); + robotMap[id] = std::make_unique(teamSpecs[id], m_data->dynamicsWorld, + btVector3(x, side * y, 0), 0.f); + robotMap[id]->setDribbleMode(m_data->dribblePerfect); + y -= 0.3; } } @@ -656,12 +483,13 @@ void Simulator::moveBall(const sslsim::TeleportBall &ball) // remove the dribbling constraint if (!ball.has_by_force() || !ball.by_force()) { - for (const auto &robotList : {m_data->robotsBlue, m_data->robotsYellow}) + for (auto &[robotId, robot] : m_data->robotsBlue) { - for (const auto &it : robotList) - { - it.first->stopDribbling(); - } + robot->stopDribbling(); + } + for (auto &[robotId, robot] : m_data->robotsYellow) + { + robot->stopDribbling(); } } @@ -690,12 +518,11 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot) if (!robot.id().has_id()) return; - bool is_blue = robot.id().team() == gameController::Team::BLUE; - - RobotMap &list = is_blue ? m_data->robotsBlue : m_data->robotsYellow; - bool isPresent = list.contains(robot.id().id()); - QMap &teamSpecs = - is_blue ? m_data->specsBlue : m_data->specsYellow; + bool isBlue = robot.id().team() == gameController::Team::BLUE; + RobotMap &robotMap = isBlue ? m_data->robotsBlue : m_data->robotsYellow; + bool isPresent = robotMap.contains(robot.id().id()); + std::map &teamSpecs = + isBlue ? m_data->specsBlue : m_data->specsYellow; if (robot.has_present()) { @@ -704,39 +531,31 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot) // add the requested robot if (!teamSpecs.contains(robot.id().id())) { - SSLSimError error{new sslsim::SimulatorError}; - error->set_code("CREATE_UNSPEC_ROBOT"); - std::string message = - "trying to create robot " + std::to_string(robot.id().id()); - message += ", but no spec for this robot was found"; - error->set_message(std::move(message)); - m_aggregator->aggregate(error, ErrorSource::CONFIG); + std::cerr << "Trying to create robot " << std::to_string(robot.id().id()) + << ", but no spec for this robot was found" << std::endl; } else if (!robot.has_x() || !robot.has_y()) { - SSLSimError error{new sslsim::SimulatorError}; - error->set_code("CREATE_NOPOS_ROBOT"); - std::string message = - "trying to create robot " + std::to_string(robot.id().id()); - message += " without giving a position"; - error->set_message(std::move(message)); - m_aggregator->aggregate(error, ErrorSource::CONFIG); + std::cerr << "Trying to create robot " << std::to_string(robot.id().id()) + << ", without giving a position" << std::endl; } else { ErForceVector targetPos; coordinates::fromVision(robot, targetPos); // TODO: check if the given position is fine - createRobot(list, targetPos.x, targetPos.y, robot.id().id(), m_aggregator, - m_data, teamSpecs); + + robotMap[robot.id().id()] = std::make_unique( + teamSpecs[robot.id().id()], m_data->dynamicsWorld, + btVector3(targetPos.x, targetPos.y, 0), 0.f); + robotMap[robot.id().id()]->setDribbleMode(m_data->dribblePerfect); } } else if (!robot.present() && isPresent) { // remove the robot - auto val = list.take(robot.id().id()); - val.first->stopDribbling(); - delete val.first; + robotMap.at(robot.id().id())->stopDribbling(); + robotMap.erase(robot.id().id()); return; } else if (!robot.present() && !isPresent) @@ -752,8 +571,8 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot) return; } - if (!list.contains(robot.id().id())) - return; // Recheck the list in case the has_present paragraph did change it. + if (!robotMap.contains(robot.id().id())) + return; // Recheck the robotMap in case the has_present paragraph did change it. sslsim::TeleportRobot r = robot; @@ -765,17 +584,11 @@ void Simulator::moveRobot(const sslsim::TeleportRobot &robot) FLIP(r, v_y); } - SimRobot *sim_robot = list[robot.id().id()].first; if (!r.has_by_force() || !r.by_force()) { - sim_robot->stopDribbling(); + robotMap[robot.id().id()]->stopDribbling(); } - sim_robot->move(r); -} - -void Simulator::setFlipped(bool flipped) -{ - m_data->flip = flipped; + robotMap[robot.id().id()]->move(r); } void Simulator::handleSimulatorSetupCommand(const std::unique_ptr &command) @@ -785,6 +598,7 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr if (command->has_simulator()) { const amun::CommandSimulator &sim = command->simulator(); + if (sim.has_enable()) { m_enabled = sim.enable(); @@ -793,6 +607,7 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr if (sim.has_realism_config()) { auto realism = sim.realism_config(); + if (realism.has_stddev_ball_p()) { m_data->stddevBall = realism.stddev_ball_p(); @@ -860,13 +675,12 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr if (realism.has_vision_delay()) { - m_visionDelay = std::max((qint64)0, (qint64)realism.vision_delay()); + m_visionDelay = std::max(0l, realism.vision_delay()); } if (realism.has_vision_processing_time()) { - m_visionProcessingTime = - std::max((qint64)0, (qint64)realism.vision_processing_time()); + m_visionProcessingTime = std::max(0l, realism.vision_processing_time()); } if (realism.has_simulate_dribbling()) @@ -914,7 +728,7 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr { if (map.contains(robot.id())) { - map[robot.id()].first->restoreState(robot); + map[robot.id()]->restoreState(robot); } } }; @@ -926,10 +740,9 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr if (command->has_transceiver()) { - const amun::CommandTransceiver &t = command->transceiver(); - if (t.has_charge()) + if (command->transceiver().has_charge()) { - m_charge = t.charge(); + m_charge = command->transceiver().charge(); } } @@ -948,73 +761,13 @@ void Simulator::handleSimulatorSetupCommand(const std::unique_ptr if (teamOrPerfectDribbleChanged) { - for (const auto &robotList : {m_data->robotsBlue, m_data->robotsYellow}) + for (auto &[robotId, robot] : m_data->robotsBlue) { - for (const auto &it : robotList) - { - SimRobot *robot = it.first; - robot->setDribbleMode(m_data->dribblePerfect); - } + robot->setDribbleMode(m_data->dribblePerfect); } - } -} - -void Simulator::seedPRGN(uint32_t seed) -{ - m_data->rng.seed(seed); -} - -static bool overlapCheck(const btVector3 &p0, const float &r0, const btVector3 &p1, - const float &r1) -{ - const float distance = (p1 - p0).length(); - return distance <= r0 + r1; -} - -// uses the real world scale -void Simulator::teleportRobotToFreePosition(SimRobot *robot) -{ - btVector3 robotPos = robot->position() / SIMULATOR_SCALE; - btVector3 direction = - (robotPos - m_data->ball->position() / SIMULATOR_SCALE).normalize(); - float distance = - 2.0f * (static_cast(BALL_MAX_RADIUS_METERS) + robot->specs().radius()); - bool valid = true; - do - { - valid = true; - robotPos = robotPos + 2 * direction * distance; - - for (const auto &robotList : {m_data->robotsBlue, m_data->robotsYellow}) + for (auto &[robotId, robot] : m_data->robotsYellow) { - for (const auto &it : robotList) - { - SimRobot *robot2 = it.first; - if (robot == robot2) - { - continue; - } - - btVector3 tmp = robot2->position() / SIMULATOR_SCALE; - if (overlapCheck(robotPos, robot->specs().radius(), tmp, - robot2->specs().radius())) - { - valid = false; - break; - } - } - if (!valid) - { - break; - } + robot->setDribbleMode(m_data->dribblePerfect); } - } while (!valid); - - sslsim::TeleportRobot robotCommand; - robotCommand.mutable_id()->set_id(robot->specs().id()); - coordinates::toVision(robotPos, robotCommand); - - robotCommand.set_v_x(0); - robotCommand.set_v_y(0); - robot->move(robotCommand); + } } diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simulator.h b/src/extlibs/er_force_sim/src/amun/simulator/simulator.h index f9cbb77432..48d3560c15 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simulator.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simulator.h @@ -21,19 +21,16 @@ #ifndef SIMULATOR_H #define SIMULATOR_H -#include -#include -#include -#include -#include +#include #include -#include +#include -#include "extlibs/er_force_sim/src/protobuf/command.h" -#include "extlibs/er_force_sim/src/protobuf/sslsim.h" +#include "extlibs/er_force_sim/src/core/rng.h" #include "proto/ssl_simulation_robot_control.pb.h" #include "proto/ssl_vision_wrapper.pb.h" - +#include "simball.h" +#include "simfield.h" +#include "simrobot.h" // higher values break the rolling friction of the ball const float SIMULATOR_SCALE = 10.0f; @@ -41,37 +38,19 @@ const float SUB_TIMESTEP = 1 / 200.f; const float COLLISION_MARGIN = 0.04f; const float FOCAL_LENGTH = 390.f; - -class QByteArray; -class QTimer; -class Timer; -class SSL_GeometryFieldSize; - namespace camun { namespace simulator { - class SimRobot; class Simulator; - class ErrorAggregator; struct SimulatorData; - - enum class ErrorSource - { - BLUE, - YELLOW, - CONFIG - }; } // namespace simulator } // namespace camun -class camun::simulator::Simulator : public QObject +class camun::simulator::Simulator { - Q_OBJECT - public: - typedef QMap> - RobotMap; /*First int: ID, Second int: Generation*/ + typedef std::map> RobotMap; /** * Creates a simulator with the given set up @@ -79,22 +58,6 @@ class camun::simulator::Simulator : public QObject * @param setup the simulator set up */ explicit Simulator(const amun::SimulatorSetup &setup); - ~Simulator() override; - Simulator(const Simulator &) = delete; - Simulator &operator=(const Simulator &) = delete; - - /** - * Seeds the pseudorandom generator - * - * @param seed - */ - void seedPRGN(uint32_t seed); - - signals: - void gotPacket(const QByteArray &data, qint64 time, QString sender); - void sendRadioResponses(const QList &responses); - void sendRealData(const QByteArray &data); // sends amun::SimulatorState - void sendSSLSimError(const QList &errors, ErrorSource source); public: /** @@ -145,11 +108,6 @@ class camun::simulator::Simulator : public QObject */ void handleSimulatorSetupCommand(const std::unique_ptr &command); - public slots: - void handleRadioCommands(const SSLSimRobotControl &control, bool isBlue, - qint64 processingStart); - void setFlipped(bool flipped); - private: /** * Accepts and executes a blue or yellow robot control command @@ -161,42 +119,91 @@ class camun::simulator::Simulator : public QObject */ std::vector acceptRobotControlCommand( const SSLSimulationProto::RobotControl &control, bool isBlue); - void sendSSLSimErrorInternal(ErrorSource source); + void resetFlipped(RobotMap &robots, float side); - std::tuple, QByteArray, qint64> createVisionPacket(); - void resetVisionPackets(); void setTeam(RobotMap &list, float side, const robot::Team &team, - QMap &specs); + std::map &specs); void moveBall(const sslsim::TeleportBall &ball); void moveRobot(const sslsim::TeleportRobot &robot); - void teleportRobotToFreePosition(SimRobot *robot); - void initializeDetection(SSLProto::SSL_DetectionFrame *detection, - std::size_t cameraId); + void initializeDetection(SSLProto::SSL_DetectionFrame &detection, size_t cameraId); private: - typedef std::tuple RadioCommand; - SimulatorData *m_data; - QQueue m_radioCommands; - QQueue, QByteArray, qint64>> m_visionPackets; - QQueue m_visionTimers; - QTimer *m_trigger; - qint64 m_time; - qint64 m_lastSentStatusTime; + std::unique_ptr m_data; + bool m_enabled; bool m_charge; - // systemDelay + visionProcessingTime = visionDelay - qint64 m_visionDelay; - qint64 m_visionProcessingTime; - - qint64 m_minRobotDetectionTime = 0; - qint64 m_minBallDetectionTime = 0; - qint64 m_lastBallSendTime = 0; - std::map m_lastFrameNumber; - ErrorAggregator *m_aggregator; + int64_t m_time; + int64_t m_visionDelay; // systemDelay + visionProcessingTime = visionDelay + int64_t m_visionProcessingTime; + int64_t m_minRobotDetectionTime = 0; + int64_t m_minBallDetectionTime = 0; + int64_t m_lastBallSendTime = 0; + std::map m_lastFrameNumber; std::mt19937 rand_shuffle_src = std::mt19937(std::random_device()()); }; + +/* Friction and restitution between robots, ball and field: (empirical + * measurments) Ball vs. Robot: Restitution: about 0.60 Friction: trial and + * error in simulator 0.18 (similar results as in reality) + * + * Ball vs. Floor: + * Restitution: sqrt(h'/h) = sqrt(0.314) = 0.56 + * Friction: \mu_k = -a / g (while slipping) = 0.35 + * + * Robot vs. Floor: + * Restitution and Friction should be as low as possible + * + * Calculations: + * Variables: r: restitution, f: friction + * Indices: b: ball; f: floor; r: robot + * + * r_b * r_f = 0.56 + * r_b * r_r = 0.60 + * r_f * r_r = small + * => r_b = 1; r_f = 0.56; r_r = 0.60 + * + * f_b * f_f = 0.35 + * f_b * f_r = 0.22 + * f_f * f_r = very small + * => f_b = 1; f_f = 0.35; f_r = 0.22 + */ + +struct camun::simulator::SimulatorData +{ + RNG rng; + std::unique_ptr collision; + std::unique_ptr dispatcher; + std::unique_ptr overlappingPairCache; + std::unique_ptr solver; + std::shared_ptr dynamicsWorld; + world::Geometry geometry; + std::vector reportedCameraSetup; + std::vector cameraPositions; + std::unique_ptr field; + std::shared_ptr ball; + Simulator::RobotMap robotsBlue; + Simulator::RobotMap robotsYellow; + std::map specsBlue; + std::map specsYellow; + bool flip; + float stddevBall; + float stddevBallArea; + float stddevRobot; + float stddevRobotPhi; + float ballDetectionsAtDribbler; // per robot per second + bool enableInvisibleBall; + float ballVisibilityThreshold; + float cameraOverlap; + float cameraPositionError; + float objectPositionOffset; + float robotCommandPacketLoss; + float robotReplyPacketLoss; + float missingBallDetections; + bool dribblePerfect; +}; + #endif // SIMULATOR_H diff --git a/src/extlibs/er_force_sim/src/core/BUILD b/src/extlibs/er_force_sim/src/core/BUILD index 517319b07a..475d773201 100644 --- a/src/extlibs/er_force_sim/src/core/BUILD +++ b/src/extlibs/er_force_sim/src/core/BUILD @@ -1,16 +1,9 @@ -load("@bazel_rules_qt//:qt.bzl", "qt_cc_library") - package(default_visibility = ["//visibility:public"]) -qt_cc_library( - name = "core_qt", +cc_library( + name = "core", srcs = glob( ["*.cpp"], ), hdrs = glob(["*.h"]), - deps = [ - "@qt//:qt_core", - "@qt//:qt_gui", - "@qt//:qt_widgets", - ], ) diff --git a/src/extlibs/er_force_sim/src/core/configuration.h b/src/extlibs/er_force_sim/src/core/configuration.h deleted file mode 100644 index 0cfb4fe8a7..0000000000 --- a/src/extlibs/er_force_sim/src/core/configuration.h +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * Copyright 2021 Andreas Wendler * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#ifndef CONFIGURATION_H -#define CONFIGURATION_H - -#include -#include - -#include -#include -#include - -#include "src/config/config.h" - -inline bool loadConfiguration(const QString &configFile, - google::protobuf::Message *message, bool allowPartial) -{ - QString fullFilename = QString(ERFORCE_CONFDIR) + configFile + ".txt"; - QFile file(fullFilename); - if (!file.open(QFile::ReadOnly)) - { - std::cout << "Could not open configuration file " << fullFilename.toStdString() - << std::endl; - return false; - } - QString str = file.readAll(); - file.close(); - std::string s = qPrintable(str); - - google::protobuf::TextFormat::Parser parser; - parser.AllowPartialMessage(allowPartial); - parser.ParseFromString(s, message); - return true; -} - -#endif // CONFIGURATION_H diff --git a/src/extlibs/er_force_sim/src/core/protobuffilereader.cpp b/src/extlibs/er_force_sim/src/core/protobuffilereader.cpp deleted file mode 100644 index 80e9f00f62..0000000000 --- a/src/extlibs/er_force_sim/src/core/protobuffilereader.cpp +++ /dev/null @@ -1,62 +0,0 @@ -/*************************************************************************** - * Copyright 2020 Andreas Wendler * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#include "protobuffilereader.h" - -ProtobufFileReader::ProtobufFileReader() : m_stream(&m_file) {} - -bool ProtobufFileReader::open(QString filename, QString filePrefix) -{ - m_file.setFileName(filename); - if (!m_file.open(QIODevice::ReadOnly)) - { - return false; - } - - // ensure compatibility across qt versions - m_stream.setVersion(QDataStream::Qt_4_6); - - QString fileType; - int version; - m_stream >> fileType >> version; - - if (fileType != filePrefix || version != 0) - { - return false; - } - - return true; -} - -bool ProtobufFileReader::readNext(google::protobuf::Message &message) -{ - if (m_stream.atEnd()) - { - return false; - } - QByteArray data; - m_stream >> data; - - if (!message.ParseFromArray(data.data(), data.size())) - { - return false; - } - return true; -} diff --git a/src/extlibs/er_force_sim/src/core/protobuffilereader.h b/src/extlibs/er_force_sim/src/core/protobuffilereader.h deleted file mode 100644 index 957c2e4a20..0000000000 --- a/src/extlibs/er_force_sim/src/core/protobuffilereader.h +++ /dev/null @@ -1,44 +0,0 @@ -/*************************************************************************** - * Copyright 2020 Andreas Wendler * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#ifndef PROTOBUFFILEREADER_H -#define PROTOBUFFILEREADER_H - -#include - -#include -#include - -class ProtobufFileReader : public QObject -{ - public: - ProtobufFileReader(); - - bool open(QString filename, QString filePrefix); - - // returns true if the message has be sucessfully parsed - bool readNext(google::protobuf::Message &message); - - private: - QFile m_file; - QDataStream m_stream; -}; - -#endif // PROTOBUFFILEREADER_H diff --git a/src/extlibs/er_force_sim/src/core/protobuffilesaver.cpp b/src/extlibs/er_force_sim/src/core/protobuffilesaver.cpp deleted file mode 100644 index 87a3a8c5db..0000000000 --- a/src/extlibs/er_force_sim/src/core/protobuffilesaver.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/*************************************************************************** - * Copyright 2020 Andreas Wendler * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#include "protobuffilesaver.h" - -#include - -ProtobufFileSaver::ProtobufFileSaver(QString filename, QString filePrefix, - QObject *parent) - : QObject(parent), - m_filename(filename), - m_filePrefix(filePrefix), - m_stream(&m_file), - m_mutex(QMutex::Recursive) -{ -} - -void ProtobufFileSaver::open() -{ - if (m_file.isOpen()) - { - return; - } - m_file.setFileName(m_filename); - if (!m_file.open(QIODevice::WriteOnly | QIODevice::Truncate)) - { - qDebug() << "Could not open path input file for saving"; - return; - } - - // ensure compatibility across qt versions - m_stream.setVersion(QDataStream::Qt_4_6); - - m_stream << QString(m_filePrefix); - m_stream << (int)0; // file version -} - -void ProtobufFileSaver::saveMessage(const google::protobuf::Message &message) -{ - QByteArray data; - data.resize(message.ByteSize()); - if (!message.IsInitialized() || !message.SerializeToArray(data.data(), data.size())) - { - return; - } - - QMutexLocker locker(&m_mutex); - open(); - - m_stream << data; -} diff --git a/src/extlibs/er_force_sim/src/core/protobuffilesaver.h b/src/extlibs/er_force_sim/src/core/protobuffilesaver.h deleted file mode 100644 index 75d16c10ff..0000000000 --- a/src/extlibs/er_force_sim/src/core/protobuffilesaver.h +++ /dev/null @@ -1,53 +0,0 @@ -/*************************************************************************** - * Copyright 2020 Andreas Wendler * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#ifndef PROTOBUFFILESAVER_H -#define PROTOBUFFILESAVER_H - -#include - -#include -#include -#include -#include -#include - -class ProtobufFileSaver : QObject -{ - Q_OBJECT - public: - // The file is created once saveMessage is called for the first time - ProtobufFileSaver(QString filename, QString filePrefix, QObject *parent = nullptr); - - // may be called from any thread - void saveMessage(const google::protobuf::Message &message); - - private: - void open(); - - private: - QString m_filename; - QString m_filePrefix; - QFile m_file; - QDataStream m_stream; - QMutex m_mutex; -}; - -#endif // PROTOBUFFILESAVER_H diff --git a/src/extlibs/er_force_sim/src/core/rng.h b/src/extlibs/er_force_sim/src/core/rng.h index b3f858f93d..319cb4566a 100644 --- a/src/extlibs/er_force_sim/src/core/rng.h +++ b/src/extlibs/er_force_sim/src/core/rng.h @@ -52,12 +52,12 @@ class RNG */ inline double RNG::uniform() { - return uniformInt() / 4294967296.0; + return static_cast(uniformInt()) / 4294967296.0; } inline float RNG::uniformFloat(float min, float max) { - return min + (uniformInt() / 4294967296.0f) * (max - min); + return min + (static_cast(uniformInt()) / 4294967296.0f) * (max - min); } /*! @@ -68,7 +68,7 @@ inline float RNG::uniformFloat(float min, float max) */ inline ErForceVector RNG::uniformVector() { - return ErForceVector(uniform(), uniform()); + return ErForceVector(static_cast(uniform()), static_cast(uniform())); } /*! diff --git a/src/extlibs/er_force_sim/src/protobuf/BUILD b/src/extlibs/er_force_sim/src/protobuf/BUILD index 1d28897496..e8f3dc17f0 100644 --- a/src/extlibs/er_force_sim/src/protobuf/BUILD +++ b/src/extlibs/er_force_sim/src/protobuf/BUILD @@ -1,5 +1,3 @@ -load("@bazel_rules_qt//:qt.bzl", "qt_cc_library") - package(default_visibility = ["//visibility:public"]) load("@rules_proto//proto:defs.bzl", "proto_library") @@ -35,16 +33,13 @@ py_proto_library( ], ) -qt_cc_library( - name = "protobuf_qt", +cc_library( + name = "protobuf", srcs = glob(["*.cpp"]), hdrs = glob(["*.h"]), linkstatic = True, deps = [ ":protobuf_cc_lib", "//proto:ssl_simulation_cc_proto", - "@qt//:qt_core", - "@qt//:qt_gui", - "@qt//:qt_widgets", ], ) diff --git a/src/extlibs/er_force_sim/src/protobuf/command.cpp b/src/extlibs/er_force_sim/src/protobuf/command.cpp deleted file mode 100644 index ae8cbaaece..0000000000 --- a/src/extlibs/er_force_sim/src/protobuf/command.cpp +++ /dev/null @@ -1,54 +0,0 @@ -/*************************************************************************** - * Copyright 2021 Andreas Wendler * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#include "command.h" - -#include "geometry.h" - -SSL_GeometryCameraCalibration createDefaultCamera(int cameraId, float x, float y, float z) -{ - SSL_GeometryCameraCalibration calibration; - - calibration.set_camera_id(cameraId); - // DUMMY VALUES - calibration.set_distortion(0.2); - calibration.set_focal_length(390); - calibration.set_principal_point_x(300); - calibration.set_principal_point_y(300); - calibration.set_q0(0.7); - calibration.set_q1(0.7); - calibration.set_q2(0.7); - calibration.set_q3(0.7); - calibration.set_tx(0); - calibration.set_ty(0); - calibration.set_tz(3500); - - calibration.set_derived_camera_world_tx(y * 1000); - calibration.set_derived_camera_world_ty(-x * 1000); - calibration.set_derived_camera_world_tz(z * 1000); - - return calibration; -} - -void simulatorSetupSetDefault(amun::SimulatorSetup &setup) -{ - geometrySetDefault(setup.mutable_geometry(), true); - setup.add_camera_setup()->CopyFrom(createDefaultCamera(0, 0.0f, 0.0f, 4.0f)); -} diff --git a/src/extlibs/er_force_sim/src/protobuf/command.h b/src/extlibs/er_force_sim/src/protobuf/command.h deleted file mode 100644 index 0a72807e68..0000000000 --- a/src/extlibs/er_force_sim/src/protobuf/command.h +++ /dev/null @@ -1,40 +0,0 @@ -/*************************************************************************** - * Copyright 2015 Philipp Nordhus * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#ifndef COMMAND_H -#define COMMAND_H - -#include - -#include "extlibs/er_force_sim/src/protobuf/command.pb.h" - -//! @file command.h -//! @addtogroup protobuf -//! @{ - -void simulatorSetupSetDefault(amun::SimulatorSetup &setup); - -// position is in meters in our coordinate system -SSL_GeometryCameraCalibration createDefaultCamera(int cameraId, float x, float y, - float z); - -//! @} - -#endif // COMMAND_H diff --git a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.cpp b/src/extlibs/er_force_sim/src/protobuf/ssl_referee.cpp deleted file mode 100644 index 25bf123c5b..0000000000 --- a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/*************************************************************************** - * Copyright 2015 Michael Eischer * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#include "ssl_referee.h" - -/*! - * \brief Initializes team info with default values - * \param teamInfo The team info to initialize - */ -void teamInfoSetDefault(SSLProto::Referee::TeamInfo *teamInfo) -{ - teamInfo->set_name(""); - teamInfo->set_score(0); - teamInfo->set_red_cards(0); - teamInfo->set_yellow_cards(0); - teamInfo->set_timeouts(4); - teamInfo->set_timeout_time(5 * 60 * 1000 * 1000); - teamInfo->set_goalkeeper(0); - assert(teamInfo->IsInitialized()); -} diff --git a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.h b/src/extlibs/er_force_sim/src/protobuf/ssl_referee.h deleted file mode 100644 index c2b498636d..0000000000 --- a/src/extlibs/er_force_sim/src/protobuf/ssl_referee.h +++ /dev/null @@ -1,28 +0,0 @@ -/*************************************************************************** - * Copyright 2015 Michael Eischer * - * Robotics Erlangen e.V. * - * http://www.robotics-erlangen.de/ * - * info@robotics-erlangen.de * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * You should have received a copy of the GNU General Public License * - * along with this program. If not, see . * - ***************************************************************************/ - -#ifndef SSL_REFEREE_H -#define SSL_REFEREE_H - -#include "proto/ssl_gc_referee_message.pb.h" - -void teamInfoSetDefault(SSLProto::Referee::TeamInfo *teamInfo); - -#endif // SSL_REFEREE_H diff --git a/src/extlibs/er_force_sim/src/protobuf/world.proto b/src/extlibs/er_force_sim/src/protobuf/world.proto index 65610f5810..1f0402f60d 100644 --- a/src/extlibs/er_force_sim/src/protobuf/world.proto +++ b/src/extlibs/er_force_sim/src/protobuf/world.proto @@ -39,15 +39,6 @@ message Geometry optional Division division = 19 [default = A]; } -message DivisionDimensions -{ - required float field_width_a = 1; - required float field_height_a = 2; - - required float field_width_b = 3; - required float field_height_b = 4; -} - message BallPosition { required int64 time = 1; diff --git a/src/extlibs/jetson_nano_gcc.BUILD b/src/extlibs/k8_jetson_nano_cross_compile_gcc.BUILD similarity index 100% rename from src/extlibs/jetson_nano_gcc.BUILD rename to src/extlibs/k8_jetson_nano_cross_compile_gcc.BUILD diff --git a/src/extlibs/linux_aarch64_gcc.BUILD b/src/extlibs/linux_aarch64_gcc.BUILD new file mode 100644 index 0000000000..fa986c6923 --- /dev/null +++ b/src/extlibs/linux_aarch64_gcc.BUILD @@ -0,0 +1,28 @@ +package(default_visibility = ["//visibility:public"]) + +filegroup( + name = "libs", + srcs = glob(["usr/lib/gcc/aarch64-linux-gnu/10/*.a"]), +) + +filegroup( + name = "includes", + srcs = glob([ + "usr/lib/gcc/aarch64-linux-gnu/10/include/**", + "usr/lib/gcc/aarch64-linux-gnu/10/include", + ]), +) + +filegroup( + name = "runtime_libs", + srcs = [ + "usr/lib/gcc/aarch64-linux-gnu/10/libstdc++.so", + ], +) + +filegroup( + name = "static_libs", + srcs = [ + "usr/lib/gcc/aarch64-linux-gnu/10/libstdc++.a", + ], +) diff --git a/src/extlibs/linux_gcc.BUILD b/src/extlibs/linux_gcc.BUILD deleted file mode 100644 index 0191adbe7e..0000000000 --- a/src/extlibs/linux_gcc.BUILD +++ /dev/null @@ -1,28 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -filegroup( - name = "libs", - srcs = glob(["usr/lib/gcc/x86_64-linux-gnu/9/*.a"]), -) - -filegroup( - name = "includes", - srcs = glob([ - "usr/lib/gcc/x86_64-linux-gnu/9/include/**", - "usr/lib/gcc/x86_64-linux-gnu/9/include", - ]), -) - -filegroup( - name = "runtime_libs", - srcs = [ - "usr/lib/gcc/x86_64-linux-gnu/9/libstdc++.so", - ], -) - -filegroup( - name = "static_libs", - srcs = [ - "usr/lib/gcc/x86_64-linux-gnu/9/libstdc++.a", - ], -) diff --git a/src/extlibs/linux_k8_gcc.BUILD b/src/extlibs/linux_k8_gcc.BUILD new file mode 100644 index 0000000000..5d4840e28c --- /dev/null +++ b/src/extlibs/linux_k8_gcc.BUILD @@ -0,0 +1,28 @@ +package(default_visibility = ["//visibility:public"]) + +filegroup( + name = "libs", + srcs = glob(["usr/lib/gcc/x86_64-linux-gnu/10/*.a"]), +) + +filegroup( + name = "includes", + srcs = glob([ + "usr/lib/gcc/x86_64-linux-gnu/10/include/**", + "usr/lib/gcc/x86_64-linux-gnu/10/include", + ]), +) + +filegroup( + name = "runtime_libs", + srcs = [ + "usr/lib/gcc/x86_64-linux-gnu/10/libstdc++.so", + ], +) + +filegroup( + name = "static_libs", + srcs = [ + "usr/lib/gcc/x86_64-linux-gnu/10/libstdc++.a", + ], +) diff --git a/src/extlibs/nanopb_requirements_lock.txt b/src/extlibs/nanopb_requirements_lock.txt index fad7c9e32e..13481a4ab1 100644 --- a/src/extlibs/nanopb_requirements_lock.txt +++ b/src/extlibs/nanopb_requirements_lock.txt @@ -1,5 +1,5 @@ # -# This file is autogenerated by pip-compile with Python 3.10 +# This file is autogenerated by pip-compile with Python 3.12 # by the following command: # # bazel run //extlibs:nanopb_requirements.update diff --git a/src/extlibs/tracy.BUILD b/src/extlibs/tracy.BUILD index 8ed4c5fa2a..de660aba65 100644 --- a/src/extlibs/tracy.BUILD +++ b/src/extlibs/tracy.BUILD @@ -12,5 +12,6 @@ cc_library( ], ), includes = ["public/tracy"], + linkopts = ["-ldl"], visibility = ["//visibility:public"], ) diff --git a/src/proto/BUILD b/src/proto/BUILD index a83817ed9a..9557086590 100644 --- a/src/proto/BUILD +++ b/src/proto/BUILD @@ -93,6 +93,7 @@ proto_library( deps = [ "@com_google_protobuf//:duration_proto", "@com_google_protobuf//:timestamp_proto", + "@com_google_protobuf//:wrappers_proto", ], ) diff --git a/src/proto/message_translation/ssl_simulation_robot_control.cpp b/src/proto/message_translation/ssl_simulation_robot_control.cpp index fe87dde214..7f1d7d998f 100644 --- a/src/proto/message_translation/ssl_simulation_robot_control.cpp +++ b/src/proto/message_translation/ssl_simulation_robot_control.cpp @@ -16,10 +16,12 @@ std::unique_ptr createRobotMoveCommand( { switch (direct_control.motor_control().drive_control_case()) { + case TbotsProto::MotorControl::DRIVE_CONTROL_NOT_SET: + break; + case TbotsProto::MotorControl::kDirectPerWheelControl: - { LOG(FATAL) << "Direct per-wheel control is not supported in simulation"; - } + break; case TbotsProto::MotorControl::kDirectVelocityControl: { diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index bc5623b8c5..0f780eccd9 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -321,8 +321,8 @@ std::unique_ptr createPlotJugglerValue( { auto plot_juggler_value_msg = std::make_unique(); double now = - static_cast(std::chrono::system_clock::now().time_since_epoch().count() / - NANOSECONDS_PER_SECOND); + static_cast(std::chrono::system_clock::now().time_since_epoch().count()) / + NANOSECONDS_PER_SECOND; plot_juggler_value_msg->set_timestamp(now); for (auto const& [key, val] : values) { @@ -479,8 +479,8 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( robot_constants.robot_max_ang_acceleration_rad_per_s_2)); } -double convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants_t robot_constants) +int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, + RobotConstants_t robot_constants) { switch (dribbler_mode) { @@ -489,10 +489,10 @@ double convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode case TbotsProto::DribblerMode::MAX_FORCE: return robot_constants.max_force_dribbler_speed_rpm; case TbotsProto::DribblerMode::OFF: - return 0.0; + return 0; default: LOG(WARNING) << "DribblerMode is invalid" << std::endl; - return 0.0; + return 0; } } diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 15f8cef4ed..50ab53c374 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -267,8 +267,8 @@ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( * * @return the dribbler speed in RPM */ -double convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, - RobotConstants_t robot_constants); +int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode, + RobotConstants_t robot_constants); /** * Convert max allowed speed mode to max allowed speed diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index d64cb0708d..9cdde04170 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -74,31 +74,34 @@ class TbotsProtobufTest : public ::testing::Test auto traj_path_nodes_1 = traj_path_1.getTrajectoryPathNodes(); auto traj_path_nodes_2 = traj_path_2.getTrajectoryPathNodes(); ASSERT_EQ(traj_path_nodes_1.size(), traj_path_nodes_2.size()); - ASSERT_FLOAT_EQ(traj_path_nodes_1[0].getTrajectoryEndTime(), - traj_path_nodes_2[0].getTrajectoryEndTime()); - for (int i = 0; i < traj_path_nodes_1.size(); i++) + // In some cases, the trajectory end time may have lost precision when converting + // between doubles and floats + ASSERT_NEAR(traj_path_nodes_1[0].getTrajectoryEndTime(), + traj_path_nodes_2[0].getTrajectoryEndTime(), 0.000001); + + for (std::size_t i = 0; i < traj_path_nodes_1.size(); i++) { EXPECT_EQ(traj_path_nodes_1[i].getTrajectory()->getPosition(0.0), traj_path_nodes_2[i].getTrajectory()->getPosition(0.0)) << " Position at index " << i << " is not equal"; } - for (int i = 0; i < traj_path_nodes_1.size(); i++) + for (std::size_t i = 0; i < traj_path_nodes_1.size(); i++) { EXPECT_EQ(traj_path_nodes_1[i].getTrajectory()->getVelocity(0.0), traj_path_nodes_2[i].getTrajectory()->getVelocity(0.0)) << " Velocity at index " << i << " is not equal"; } - for (int i = 0; i < traj_path_nodes_1.size(); i++) + for (std::size_t i = 0; i < traj_path_nodes_1.size(); i++) { EXPECT_EQ(traj_path_nodes_1[i].getTrajectory()->getAcceleration(0.0), traj_path_nodes_2[i].getTrajectory()->getAcceleration(0.0)) << " Acceleration at index " << i << " is not equal"; } - for (int i = 0; i < traj_path_nodes_1.size(); i++) + for (std::size_t i = 0; i < traj_path_nodes_1.size(); i++) { EXPECT_EQ(traj_path_nodes_1[i].getTrajectory()->getDestination(), traj_path_nodes_2[i].getTrajectory()->getDestination()) @@ -176,7 +179,7 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) start_position, initial_destination, initial_velocity, constraints); TrajectoryPath trajectory_path(trajectory, BangBangTrajectory2D::generator); - for (int i = 1; i < sub_destinations.size(); i++) + for (std::size_t i = 1; i < sub_destinations.size(); i++) { trajectory_path.append(sub_destination_connection_times[i - 1], sub_destinations[i], constraints); @@ -195,12 +198,13 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) *(params.mutable_initial_velocity()) = *createVectorProto(initial_velocity); params.set_max_speed_mode(max_allowed_speed_mode); - for (int i = 0; i < sub_destinations.size(); ++i) + for (std::size_t i = 0; i < sub_destinations.size(); ++i) { TbotsProto::TrajectoryPathParams2D::SubDestination sub_destination_proto; *(sub_destination_proto.mutable_sub_destination()) = *createPointProto(sub_destinations[i]); - sub_destination_proto.set_connection_time_s(sub_destination_connection_times[i]); + sub_destination_proto.set_connection_time_s( + static_cast(sub_destination_connection_times[i])); *(params.add_sub_destinations()) = sub_destination_proto; } diff --git a/src/proto/primitive.proto b/src/proto/primitive.proto index b679a65127..c2408c8955 100644 --- a/src/proto/primitive.proto +++ b/src/proto/primitive.proto @@ -92,12 +92,6 @@ message PowerControl }; } - message GenevaControl - { - float angle_deg = 1; - float rotation_speed_rpm = 2; - } - ChickerControl chicker = 1; Geneva.Slot geneva_slot = 2; } diff --git a/src/proto/primitive/primitive_msg_factory.cpp b/src/proto/primitive/primitive_msg_factory.cpp index d1cd2ecc3f..5aef38a878 100644 --- a/src/proto/primitive/primitive_msg_factory.cpp +++ b/src/proto/primitive/primitive_msg_factory.cpp @@ -13,7 +13,7 @@ std::unique_ptr createStopPrimitiveProto() } std::unique_ptr createDirectControlPrimitive( - const Vector &velocity, AngularVelocity angular_velocity, double dribbler_speed_rpm, + const Vector &velocity, AngularVelocity angular_velocity, int dribbler_speed_rpm, const TbotsProto::AutoChipOrKick &auto_chip_or_kick) { auto direct_control_primitive_msg = std::make_unique(); @@ -30,7 +30,7 @@ std::unique_ptr createDirectControlPrimitive( direct_control_primitive_msg->mutable_direct_control() ->mutable_motor_control() - ->set_dribbler_speed_rpm(static_cast(dribbler_speed_rpm)); + ->set_dribbler_speed_rpm(dribbler_speed_rpm); *(direct_control_primitive_msg->mutable_direct_control() ->mutable_power_control() diff --git a/src/proto/primitive/primitive_msg_factory.h b/src/proto/primitive/primitive_msg_factory.h index 40378ebd18..403395b0c6 100644 --- a/src/proto/primitive/primitive_msg_factory.h +++ b/src/proto/primitive/primitive_msg_factory.h @@ -30,5 +30,5 @@ std::unique_ptr createStopPrimitiveProto(); * @return Pointer to the DirectControl Primitive */ std::unique_ptr createDirectControlPrimitive( - const Vector &velocity, AngularVelocity angular_velocity, double dribbler_rpm, + const Vector &velocity, AngularVelocity angular_velocity, int dribbler_rpm, const TbotsProto::AutoChipOrKick &auto_chip_or_kick); diff --git a/src/proto/ssl_gc_api.proto b/src/proto/ssl_gc_api.proto index 98e6db7288..1f2c3d04fc 100644 --- a/src/proto/ssl_gc_api.proto +++ b/src/proto/ssl_gc_api.proto @@ -56,4 +56,6 @@ message Input optional bool reset_match = 2; // An updated config delta optional Config config_delta = 3; + // Continue with action + optional ContinueAction continue_action = 4; } diff --git a/src/proto/ssl_gc_change.proto b/src/proto/ssl_gc_change.proto index 2b99446f67..41762b1057 100644 --- a/src/proto/ssl_gc_change.proto +++ b/src/proto/ssl_gc_change.proto @@ -9,6 +9,7 @@ import "proto/ssl_gc_game_event.proto"; import "proto/ssl_gc_referee_message.proto"; import "google/protobuf/timestamp.proto"; +import "google/protobuf/wrappers.proto"; // A state change message StateChange @@ -35,183 +36,185 @@ message Change oneof change { - NewCommand new_command = 2; - ChangeStage change_stage = 3; - SetBallPlacementPos set_ball_placement_pos = 4; - AddYellowCard add_yellow_card = 5; - AddRedCard add_red_card = 6; - YellowCardOver yellow_card_over = 7; - AddGameEvent add_game_event = 8; - AddPassiveGameEvent add_passive_game_event = 19; - AddProposal add_proposal = 9; - StartBallPlacement start_ball_placement = 10; - Continue continue = 11; - UpdateConfig update_config = 12; - UpdateTeamState update_team_state = 13; - SwitchColors switch_colors = 14; - Revert revert = 15; - NewGameState new_game_state = 17; - AcceptProposalGroup accept_proposal_group = 18; + NewCommand new_command_change = 2; + ChangeStage change_stage_change = 3; + SetBallPlacementPos set_ball_placement_pos_change = 4; + AddYellowCard add_yellow_card_change = 5; + AddRedCard add_red_card_change = 6; + YellowCardOver yellow_card_over_change = 7; + AddGameEvent add_game_event_change = 8; + AddPassiveGameEvent add_passive_game_event_change = 19; + AddProposal add_proposal_change = 9; + UpdateConfig update_config_change = 12; + UpdateTeamState update_team_state_change = 13; + SwitchColors switch_colors_change = 14; + Revert revert_change = 15; + NewGameState new_game_state_change = 17; + AcceptProposalGroup accept_proposal_group_change = 18; + SetStatusMessage set_status_message_change = 20; } -} - -// New referee command -message NewCommand -{ - // The command - optional Command command = 1; -} -// Switch to a new stage -message ChangeStage -{ - // The new stage - optional Referee.Stage new_stage = 1; -} + // New referee command + message NewCommand + { + // The command + optional Command command = 1; + } -// Set the ball placement pos -message SetBallPlacementPos -{ - // The position in [m] - optional Vector2 pos = 1; -} + // Switch to a new stage + message ChangeStage + { + // The new stage + optional Referee.Stage new_stage = 1; + } -// Add a new yellow card -message AddYellowCard -{ - // The team that the card is for - optional Team for_team = 1; - // The game event that caused the card - optional GameEvent caused_by_game_event = 2; -} + // Set the ball placement pos + message SetBallPlacementPos + { + // The position in [m] + optional Vector2 pos = 1; + } -// Add a new red card -message AddRedCard -{ - // The team that the card is for - optional Team for_team = 1; - // The game event that caused the card - optional GameEvent caused_by_game_event = 2; -} + // Add a new yellow card + message AddYellowCard + { + // The team that the card is for + optional Team for_team = 1; + // The game event that caused the card + optional GameEvent caused_by_game_event = 2; + } -// Trigger when a yellow card timed out -message YellowCardOver -{ - // The team that the card was for - optional Team for_team = 1; -} + // Add a new red card + message AddRedCard + { + // The team that the card is for + optional Team for_team = 1; + // The game event that caused the card + optional GameEvent caused_by_game_event = 2; + } -// Add a new game event -message AddGameEvent -{ - // The game event - optional GameEvent game_event = 1; -} + // Trigger when a yellow card timed out + message YellowCardOver + { + // The team that the card was for + optional Team for_team = 1; + } -// Add a new passive game event (that is only logged, but does not automatically trigger -// anything) -message AddPassiveGameEvent -{ - // The game event - optional GameEvent game_event = 1; -} + // Add a new game event + message AddGameEvent + { + // The game event + optional GameEvent game_event = 1; + } -// Add a new proposal (i.e. from an auto referee for majority voting) -message AddProposal -{ - // The proposal - optional Proposal proposal = 1; -} + // Add a new passive game event (that is only logged, but does not automatically + // trigger anything) + message AddPassiveGameEvent + { + // The game event + optional GameEvent game_event = 1; + } -// Accept a proposal group (that contain one or more proposals of the same type) -message AcceptProposalGroup -{ - // The id of the group - optional uint32 group_id = 1; - // An identifier of the acceptor - optional string accepted_by = 2; -} + // Add a new proposal (i.e. from an auto referee for majority voting) + message AddProposal + { + // The proposal + optional Proposal proposal = 1; + } -// Initiate ball placement -message StartBallPlacement {} + // Accept a proposal group (that contain one or more proposals of the same type) + message AcceptProposalGroup + { + // The id of the group + optional string group_id = 3; + // An identifier of the acceptor + optional string accepted_by = 2; + } -// Continue with the next referee command -message Continue {} + // Update some configuration + message UpdateConfig + { + // The division to play with + optional Division division = 1; + // the team that does/did the first kick off + optional Team first_kickoff_team = 2; + reserved 3; // auto_continue moved to gcState + // The match type + optional MatchType match_type = 4; + // The number of robots per team + optional google.protobuf.Int32Value max_robots_per_team = 5; + } -// Update some configuration -message UpdateConfig -{ - // The division to play with - optional Division division = 1; - // the team that does/did the first kick off - optional Team first_kickoff_team = 2; - // Enable automatic continuation when all conditions are met - optional bool auto_continue = 3; - // The match type - optional MatchType match_type = 4; -} + // Update the current state of a team (all fields that should be updated are set) + message UpdateTeamState + { + // The team + optional Team for_team = 1; + + // Change the name of the team + optional google.protobuf.StringValue team_name = 2; + // Change the number of goals that the teams has at the moment + optional google.protobuf.Int32Value goals = 3; + // The id of the goal keeper + optional google.protobuf.Int32Value goalkeeper = 4; + // The number of timeouts that the team has left + optional google.protobuf.Int32Value timeouts_left = 5; + // The timeout time that the team has left + optional google.protobuf.StringValue timeout_time_left = 6; + // Does the team play on the positive or the negative half (in ssl-vision + // coordinates)? + optional google.protobuf.BoolValue on_positive_half = 7; + // The number of ball placement failures + optional google.protobuf.Int32Value ball_placement_failures = 8; + // Can the team place the ball, or is ball placement for this team disabled and + // should be skipped? + optional google.protobuf.BoolValue can_place_ball = 9; + // The number of challenge flags that the team has left + optional google.protobuf.Int32Value challenge_flags_left = 21; + // The number of bot substitutions left by the team in this halftime + optional google.protobuf.Int32Value bot_substitutions_left = 22; + // Does the team want to substitute a robot in the next possible situation? + optional google.protobuf.BoolValue requests_bot_substitution = 10; + // Does the team want to take a timeout in the next possible situation? + optional google.protobuf.BoolValue requests_timeout = 17; + // Does the team want to challenge a recent decision of the referee? + optional google.protobuf.BoolValue requests_challenge = 18; + // Does the team want to request an emergency stop? + optional google.protobuf.BoolValue requests_emergency_stop = 19; + // Update a certain yellow card of the team + optional YellowCard yellow_card = 20; + // Update a certain red card of the team + optional RedCard red_card = 12; + // Update a certain foul of the team + optional Foul foul = 13; + // Remove the yellow card with this id + optional google.protobuf.UInt32Value remove_yellow_card = 14; + // Remove the red card with this id + optional google.protobuf.UInt32Value remove_red_card = 15; + // Remove the foul with this id + optional google.protobuf.UInt32Value remove_foul = 16; + } -// Update the current state of a team (all fields that should be updated are set) -message UpdateTeamState -{ - // The team - optional Team for_team = 1; - - // Change the name of the team - optional string team_name = 2; - // Change the number of goals that the teams has at the moment - optional int32 goals = 3; - // The id of the goal keeper - optional int32 goalkeeper = 4; - // The number of timeouts that the team has left - optional int32 timeouts_left = 5; - // The timeout time that the team has left - optional string timeout_time_left = 6; - // Does the team play on the positive or the negative half (in ssl-vision - // coordinates)? - optional bool on_positive_half = 7; - // The number of ball placement failures - optional int32 ball_placement_failures = 8; - // Can the team place the ball, or is ball placement for this team disabled and should - // be skipped? - optional bool can_place_ball = 9; - // The number of challenge flags that the team has left - optional int32 challenge_flags_left = 21; - // Does the team want to substitute a robot in the next possible situation? - optional bool requests_bot_substitution = 10; - // Does the team want to take a timeout in the next possible situation? - optional bool requests_timeout = 17; - // Does the team want to challenge a recent decision of the referee? - optional bool requests_challenge = 18; - // Does the team want to request an emergency stop? - optional bool requests_emergency_stop = 19; - // Update a certain yellow card of the team - optional YellowCard yellow_card = 20; - // Update a certain red card of the team - optional RedCard red_card = 12; - // Update a certain foul of the team - optional Foul foul = 13; - // Remove the yellow card with this id - optional uint32 remove_yellow_card = 14; - // Remove the red card with this id - optional uint32 remove_red_card = 15; - // Remove the foul with this id - optional uint32 remove_foul = 16; -} + // Switch the team colors + message SwitchColors {} -// Switch the team colors -message SwitchColors {} + // Revert a certain change + message Revert + { + // The id of the change + optional int32 change_id = 1; + } -// Revert a certain change -message Revert -{ - // The id of the change - optional int32 change_id = 1; -} + // Change the current game state + message NewGameState + { + // The new game state + optional GameState game_state = 1; + } -// Change the current game state -message NewGameState -{ - // The new game state - optional GameState game_state = 1; + message SetStatusMessage + { + // The new status message + optional string status_message = 1; + } } diff --git a/src/proto/ssl_gc_engine.proto b/src/proto/ssl_gc_engine.proto index 1b6c271021..dce2f0bceb 100644 --- a/src/proto/ssl_gc_engine.proto +++ b/src/proto/ssl_gc_engine.proto @@ -5,26 +5,25 @@ package SSLProto; import "proto/ssl_gc_geometry.proto"; import "proto/ssl_gc_common.proto"; +import "google/protobuf/timestamp.proto"; + // The GC state contains settings and state independent of the match state message GcState { - // The state of each team + // the state of each team map team_state = 1; // the states of the auto referees map auto_ref_state = 2; - // the states of the attached trackers - map tracker_state = 3; - - // the state of the currently selected tracker - optional GcStateTracker tracker_state_gc = 4; + // the attached trackers (uuid -> source_name) + map trackers = 3; - // can the match be continued right now? - optional bool ready_to_continue = 5; + // the next actions that can be executed when continuing + repeated ContinueAction continue_actions = 4; - // list of issues that hinders the game from continuing - repeated string continuation_issues = 6; + // the next actions that can be executed when continuing + repeated ContinueHint continue_hints = 5; } // The GC state for a single team @@ -104,3 +103,61 @@ message Robot // robot position [m] optional Vector2 pos = 2; } + +message ContinueAction +{ + // type of action that will be performed next + required Type type = 1; + + // for which team (if team specific) + required Team for_team = 2; + + // list of issues that hinders the game from continuing + repeated string continuation_issues = 3; + + // timestamp at which the action will be ready (to give some preparation time) + optional google.protobuf.Timestamp ready_at = 4; + + // state of the action + optional State state = 5; + + enum Type + { + TYPE_UNKNOWN = 0; + HALT = 1; + RESUME_FROM_HALT = 10; + STOP_GAME = 2; + FORCE_START = 11; + FREE_KICK = 17; + NEXT_COMMAND = 3; + BALL_PLACEMENT_START = 4; + BALL_PLACEMENT_CANCEL = 9; + BALL_PLACEMENT_COMPLETE = 14; + BALL_PLACEMENT_FAIL = 15; + TIMEOUT_START = 5; + TIMEOUT_STOP = 6; + BOT_SUBSTITUTION = 7; + NEXT_STAGE = 8; + END_GAME = 16; + ACCEPT_GOAL = 12; + REJECT_GOAL = 20; + NORMAL_START = 13; + CHALLENGE_ACCEPT = 18; + CHALLENGE_REJECT = 19; + } + + enum State + { + STATE_UNKNOWN = 0; + BLOCKED = 1; + WAITING = 2; + READY_AUTO = 3; + READY_MANUAL = 4; + DISABLED = 5; + } +} + +message ContinueHint +{ + required string message = 1; +} diff --git a/src/proto/ssl_gc_engine_config.proto b/src/proto/ssl_gc_engine_config.proto index 6a71aafdbd..5c0ef9231e 100644 --- a/src/proto/ssl_gc_engine_config.proto +++ b/src/proto/ssl_gc_engine_config.proto @@ -17,6 +17,9 @@ message Config // The list of available teams repeated string teams = 4; + // Enable or disable auto continuation + optional bool auto_continue = 5; + // Behaviors for each game event enum Behavior { diff --git a/src/proto/ssl_gc_game_event.proto b/src/proto/ssl_gc_game_event.proto index bf510a37b7..7f103ed443 100644 --- a/src/proto/ssl_gc_game_event.proto +++ b/src/proto/ssl_gc_game_event.proto @@ -15,6 +15,10 @@ import "proto/ssl_gc_geometry.proto"; // so. message GameEvent { + // A globally unique id of the game event. + optional string id = 50; + + // The type of the game event. optional Type type = 40; // The origins of this game event. @@ -23,6 +27,9 @@ message GameEvent // Ignored if sent by autoRef to game controller. repeated string origin = 41; + // Unix timestamp in microseconds when the event was created. + optional uint64 created_timestamp = 49; + // the event that occurred oneof event { @@ -43,6 +50,7 @@ message GameEvent BotPushedBot bot_pushed_bot = 24; BotHeldBallDeliberately bot_held_ball_deliberately = 26; BotTippedOver bot_tipped_over = 27; + BotDroppedParts bot_dropped_parts = 51; // Non-Stopping Fouls @@ -69,17 +77,45 @@ message GameEvent PlacementSucceeded placement_succeeded = 5; PenaltyKickFailed penalty_kick_failed = 45; - NoProgressInGame no_progress_in_game = 2; - PlacementFailed placement_failed = 3; - MultipleCards multiple_cards = 32; - MultipleFouls multiple_fouls = 34; - BotSubstitution bot_substitution = 37; - TooManyRobots too_many_robots = 38; - ChallengeFlag challenge_flag = 46; - EmergencyStop emergency_stop = 47; + NoProgressInGame no_progress_in_game = 2; + PlacementFailed placement_failed = 3; + MultipleCards multiple_cards = 32; + MultipleFouls multiple_fouls = 34; + BotSubstitution bot_substitution = 37; + ExcessiveBotSubstitution excessive_bot_substitution = 52; + TooManyRobots too_many_robots = 38; + ChallengeFlag challenge_flag = 46; + ChallengeFlagHandled challenge_flag_handled = 48; + EmergencyStop emergency_stop = 47; UnsportingBehaviorMinor unsporting_behavior_minor = 35; UnsportingBehaviorMajor unsporting_behavior_major = 36; + + // Deprecated events + + // replaced by ready_to_continue flag + Prepared prepared = 1 [deprecated = true]; + // obsolete + IndirectGoal indirect_goal = 9 [deprecated = true]; + // replaced by the meta-information in the possible_goal event + ChippedGoal chipped_goal = 10 [deprecated = true]; + // obsolete + KickTimeout kick_timeout = 12 [deprecated = true]; + // rule removed + AttackerTouchedOpponentInDefenseArea attacker_touched_opponent_in_defense_area = + 16 [deprecated = true]; + // obsolete + AttackerTouchedOpponentInDefenseArea + attacker_touched_opponent_in_defense_area_skipped = 42 [deprecated = true]; + // obsolete + BotCrashUnique bot_crash_unique_skipped = 23 [deprecated = true]; + // can not be used as long as autoRefs do not judge pushing + BotPushedBot bot_pushed_bot_skipped = 25 [deprecated = true]; + // rule removed + DefenderInDefenseAreaPartially defender_in_defense_area_partially = 30 + [deprecated = true]; + // the referee msg already indicates this + MultiplePlacementFailures multiple_placement_failures = 33 [deprecated = true]; } // the ball left the field normally @@ -89,7 +125,7 @@ message GameEvent required Team by_team = 1; // the bot that last touched the ball optional uint32 by_bot = 2; - // the location where the ball left the field + // the location where the ball left the field [m] optional Vector2 location = 3; } // the ball left the field via goal line and a team committed an aimless kick @@ -99,9 +135,9 @@ message GameEvent required Team by_team = 1; // the bot that last touched the ball optional uint32 by_bot = 2; - // the location where the ball left the field + // the location where the ball left the field [m] optional Vector2 location = 3; - // the location where the ball was last touched + // the location where the ball was last touched [m] optional Vector2 kick_location = 4; } // a team shot a goal @@ -113,12 +149,13 @@ message GameEvent optional Team kicking_team = 6; // the bot that shot the goal optional uint32 kicking_bot = 2; - // the location where the ball entered the goal + // the location where the ball entered the goal [m] optional Vector2 location = 3; // the location where the ball was kicked (for deciding if this was a valid goal) + // [m] optional Vector2 kick_location = 4; // the maximum height the ball reached during the goal kick (for deciding if this - // was a valid goal) + // was a valid goal) [m] optional float max_ball_height = 5; // number of robots of scoring team when the ball entered the goal (for deciding // if this was a valid goal) @@ -135,9 +172,9 @@ message GameEvent required Team by_team = 1; // the bot that kicked the ball - at least the team must be set optional uint32 by_bot = 2; - // the location where the ball entered the goal + // the location where the ball entered the goal [m] optional Vector2 location = 3; - // the location where the ball was kicked + // the location where the ball was kicked [m] optional Vector2 kick_location = 4; } // the ball entered the goal, but was initially chipped @@ -147,12 +184,12 @@ message GameEvent required Team by_team = 1; // the bot that kicked the ball optional uint32 by_bot = 2; - // the location where the ball entered the goal + // the location where the ball entered the goal [m] optional Vector2 location = 3; - // the location where the ball was kicked + // the location where the ball was kicked [m] optional Vector2 kick_location = 4; // the maximum height [m] of the ball, before it entered the goal and since the - // last kick + // last kick [m] optional float max_ball_height = 5; } // a bot moved too fast while the game was stopped @@ -162,7 +199,7 @@ message GameEvent required Team by_team = 1; // the bot that was too fast optional uint32 by_bot = 2; - // the location of the bot + // the location of the bot [m] optional Vector2 location = 3; // the bot speed [m/s] optional float speed = 4; @@ -174,7 +211,7 @@ message GameEvent required Team by_team = 1; // the bot that violates the distance to the kick point optional uint32 by_bot = 2; - // the location of the bot + // the location of the bot [m] optional Vector2 location = 3; // the distance [m] from bot to the kick point (including the minimum radius) optional float distance = 4; @@ -186,7 +223,7 @@ message GameEvent optional uint32 bot_yellow = 1; // the bot of the blue team optional uint32 bot_blue = 2; - // the location of the crash (center between both bots) + // the location of the crash (center between both bots) [m] optional Vector2 location = 3; // the calculated crash speed [m/s] of the two bots optional float crash_speed = 4; @@ -207,7 +244,7 @@ message GameEvent optional uint32 violator = 2; // the bot of the opposite team that was involved in the crash optional uint32 victim = 3; - // the location of the crash (center between both bots) + // the location of the crash (center between both bots) [m] optional Vector2 location = 4; // the calculated crash speed vector [m/s] of the two bots optional float crash_speed = 5; @@ -227,7 +264,7 @@ message GameEvent optional uint32 violator = 2; // the bot of the opposite team that was pushed optional uint32 victim = 3; - // the location of the push (center between both bots) + // the location of the push (center between both bots) [m] optional Vector2 location = 4; // the pushed distance [m] optional float pushed_distance = 5; @@ -239,9 +276,21 @@ message GameEvent required Team by_team = 1; // the bot that tipped over optional uint32 by_bot = 2; - // the location of the bot + // the location of the bot [m] optional Vector2 location = 3; - // the location of the ball at the moment when this foul occurred + // the location of the ball at the moment when this foul occurred [m] + optional Vector2 ball_location = 4; + } + // a bot dropped parts + message BotDroppedParts + { + // the team that found guilty + required Team by_team = 1; + // the bot that dropped the parts + optional uint32 by_bot = 2; + // the location where the parts were dropped [m] + optional Vector2 location = 3; + // the location of the ball at the moment when this foul occurred [m] optional Vector2 ball_location = 4; } // a defender other than the keeper was fully located inside its own defense and @@ -252,7 +301,7 @@ message GameEvent required Team by_team = 1; // the bot that is inside the penalty area optional uint32 by_bot = 2; - // the location of the bot + // the location of the bot [m] optional Vector2 location = 3; // the distance [m] from bot case to the nearest point outside the defense area optional float distance = 4; @@ -269,7 +318,7 @@ message GameEvent optional Vector2 location = 3; // the distance [m] that the bot is inside the penalty area optional float distance = 4; - // the location of the ball at the moment when this foul occurred + // the location of the ball at the moment when this foul occurred [m] optional Vector2 ball_location = 5; } // an attacker touched the ball inside the opponent defense area @@ -279,7 +328,7 @@ message GameEvent required Team by_team = 1; // the bot that is inside the penalty area optional uint32 by_bot = 2; - // the location of the bot + // the location of the bot [m] optional Vector2 location = 3; // the distance [m] that the bot is inside the penalty area optional float distance = 4; @@ -291,7 +340,7 @@ message GameEvent required Team by_team = 1; // the bot that kicked too fast optional uint32 by_bot = 2; - // the location of the ball at the time of the highest speed + // the location of the ball at the time of the highest speed [m] optional Vector2 location = 3; // the absolute initial ball speed (kick speed) [m/s] optional float initial_ball_speed = 4; @@ -305,9 +354,9 @@ message GameEvent required Team by_team = 1; // the bot that dribbled too far optional uint32 by_bot = 2; - // the location where the dribbling started + // the location where the dribbling started [m] optional Vector2 start = 3; - // the location where the maximum dribbling distance was reached + // the location where the maximum dribbling distance was reached [m] optional Vector2 end = 4; } // an attacker touched the opponent robot inside defense area @@ -319,7 +368,7 @@ message GameEvent optional uint32 by_bot = 2; // the bot of the opposite team that was touched optional uint32 victim = 4; - // the location of the contact point between both bots + // the location of the contact point between both bots [m] optional Vector2 location = 3; } // an attacker touched the ball multiple times when it was not allowed to @@ -329,7 +378,7 @@ message GameEvent required Team by_team = 1; // the bot that touched the ball twice optional uint32 by_bot = 2; - // the location of the ball when it was first touched + // the location of the ball when it was first touched [m] optional Vector2 location = 3; } // an attacker was located too near to the opponent defense area during stop or free @@ -340,11 +389,11 @@ message GameEvent required Team by_team = 1; // the bot that is too close to the defense area optional uint32 by_bot = 2; - // the location of the bot + // the location of the bot [m] optional Vector2 location = 3; // the distance [m] of the bot to the penalty area optional float distance = 4; - // the location of the ball at the moment when this foul occurred + // the location of the ball at the moment when this foul occurred [m] optional Vector2 ball_location = 5; } // a bot held the ball for too long @@ -354,7 +403,7 @@ message GameEvent required Team by_team = 1; // the bot that holds the ball optional uint32 by_bot = 2; - // the location of the ball + // the location of the ball [m] optional Vector2 location = 3; // the duration [s] that the bot hold the ball optional float duration = 4; @@ -366,10 +415,10 @@ message GameEvent required Team by_team = 1; // the bot that interfered the placement optional uint32 by_bot = 2; - // the location of the bot + // the location of the bot [m] optional Vector2 location = 3; } - // a team collected multiple cards (yellow and red), which results in a penalty kick + // a team collected multiple yellow cards message MultipleCards { // the team that received multiple yellow cards @@ -380,6 +429,8 @@ message GameEvent { // the team that collected multiple fouls required Team by_team = 1; + // the list of game events that caused the multiple fouls + repeated GameEvent caused_game_events = 2; } // a team failed to place the ball multiple times in a row message MultiplePlacementFailures @@ -392,7 +443,7 @@ message GameEvent { // the team that that should have kicked required Team by_team = 1; - // the location of the ball + // the location of the ball [m] optional Vector2 location = 2; // the time [s] that was waited optional float time = 3; @@ -412,6 +463,8 @@ message GameEvent required Team by_team = 1; // the remaining distance [m] from ball to placement position optional float remaining_distance = 2; + // the distance [m] of the nearest own robot to the ball + optional float nearest_own_bot_distance = 3; } // a team was found guilty for minor unsporting behavior message UnsportingBehaviorMinor @@ -434,7 +487,7 @@ message GameEvent { // the team that found guilty required Team by_team = 1; - // the location of the ball + // the location of the ball [m] optional Vector2 location = 2; // the duration [s] that the keeper hold the ball optional float duration = 3; @@ -464,12 +517,26 @@ message GameEvent // the team that substitutes robots required Team by_team = 1; } + // A foul for excessive bot substitutions + message ExcessiveBotSubstitution + { + // the team that substitutes robots + required Team by_team = 1; + } // A challenge flag, requested by a team previously, is flagged message ChallengeFlag { - // the team that substitutes robots + // the team that requested the challenge flag required Team by_team = 1; } + // A challenge, flagged recently, has been handled by the referee + message ChallengeFlagHandled + { + // the team that requested the challenge flag + required Team by_team = 1; + // the challenge was accepted by the referee + required bool accepted = 2; + } // An emergency stop, requested by team previously, occurred message EmergencyStop { @@ -485,7 +552,7 @@ message GameEvent optional int32 num_robots_allowed = 2; // number of robots currently on the field optional int32 num_robots_on_field = 3; - // the location of the ball at the moment when this foul occurred + // the location of the ball at the moment when this foul occurred [m] optional Vector2 ball_location = 4; } // a robot chipped the ball over the field boundary out of the playing surface @@ -493,7 +560,7 @@ message GameEvent { // the team that has too many robots required Team by_team = 1; - // the location of the ball + // the location of the ball [m] optional Vector2 location = 2; } // the penalty kick failed (by time or by keeper) @@ -501,8 +568,10 @@ message GameEvent { // the team that last touched the ball required Team by_team = 1; - // the location of the ball at the moment of this event + // the location of the ball at the moment of this event [m] optional Vector2 location = 2; + // an explanation of the failure + optional string reason = 3; } enum Type @@ -526,6 +595,7 @@ message GameEvent BOT_PUSHED_BOT = 24; // triggered by human ref BOT_HELD_BALL_DELIBERATELY = 26; // triggered by human ref BOT_TIPPED_OVER = 27; // triggered by human ref + BOT_DROPPED_PARTS = 47; // triggered by human ref // Non-Stopping Fouls @@ -539,6 +609,7 @@ message GameEvent DEFENDER_TOO_CLOSE_TO_KICK_POINT = 29; // triggered by autoRef BOT_TOO_FAST_IN_STOP = 28; // triggered by autoRef BOT_INTERFERED_PLACEMENT = 20; // triggered by autoRef + EXCESSIVE_BOT_SUBSTITUTION = 48; // triggered by GC // Scoring goals @@ -552,16 +623,30 @@ message GameEvent PLACEMENT_SUCCEEDED = 5; // triggered by autoRef PENALTY_KICK_FAILED = 43; // triggered by GC and autoRef - NO_PROGRESS_IN_GAME = 2; // triggered by GC - PLACEMENT_FAILED = 3; // triggered by GC - MULTIPLE_CARDS = 32; // triggered by GC - MULTIPLE_FOULS = 34; // triggered by GC - BOT_SUBSTITUTION = 37; // triggered by GC - TOO_MANY_ROBOTS = 38; // triggered by GC - CHALLENGE_FLAG = 44; // triggered by GC - EMERGENCY_STOP = 45; // triggered by GC + NO_PROGRESS_IN_GAME = 2; // triggered by GC + PLACEMENT_FAILED = 3; // triggered by GC + MULTIPLE_CARDS = 32; // triggered by GC + MULTIPLE_FOULS = 34; // triggered by GC + BOT_SUBSTITUTION = 37; // triggered by GC + TOO_MANY_ROBOTS = 38; // triggered by GC + CHALLENGE_FLAG = 44; // triggered by GC + CHALLENGE_FLAG_HANDLED = 46; // triggered by GC + EMERGENCY_STOP = 45; // triggered by GC UNSPORTING_BEHAVIOR_MINOR = 35; // triggered by human ref UNSPORTING_BEHAVIOR_MAJOR = 36; // triggered by human ref + + // Deprecated events + + PREPARED = 1 [deprecated = true]; + INDIRECT_GOAL = 9 [deprecated = true]; + CHIPPED_GOAL = 10 [deprecated = true]; + KICK_TIMEOUT = 12 [deprecated = true]; + ATTACKER_TOUCHED_OPPONENT_IN_DEFENSE_AREA = 16 [deprecated = true]; + ATTACKER_TOUCHED_OPPONENT_IN_DEFENSE_AREA_SKIPPED = 40 [deprecated = true]; + BOT_CRASH_UNIQUE_SKIPPED = 23 [deprecated = true]; + BOT_PUSHED_BOT_SKIPPED = 25 [deprecated = true]; + DEFENDER_IN_DEFENSE_AREA_PARTIALLY = 30 [deprecated = true]; + MULTIPLE_PLACEMENT_FAILURES = 33 [deprecated = true]; } } diff --git a/src/proto/ssl_gc_referee_message.proto b/src/proto/ssl_gc_referee_message.proto index 0b5540a155..24006daddc 100644 --- a/src/proto/ssl_gc_referee_message.proto +++ b/src/proto/ssl_gc_referee_message.proto @@ -75,7 +75,7 @@ message Referee // // If the stage runs over its specified time, this value // becomes negative. - optional sint32 stage_time_left = 3; + optional sint64 stage_time_left = 3; // These are the "fine" states of play on the field. enum Command @@ -101,16 +101,20 @@ message Referee // The blue team may take a direct free kick. DIRECT_FREE_BLUE = 9; // The yellow team may take an indirect free kick. - INDIRECT_FREE_YELLOW = 10; + INDIRECT_FREE_YELLOW = 10 [deprecated = true]; // The blue team may take an indirect free kick. - INDIRECT_FREE_BLUE = 11; + INDIRECT_FREE_BLUE = 11 [deprecated = true]; // The yellow team is currently in a timeout. TIMEOUT_YELLOW = 12; // The blue team is currently in a timeout. TIMEOUT_BLUE = 13; // The yellow team just scored a goal. // For information only. - // For rules compliance, teams must treat as STOP. + // Deprecated: Use the score field from the team infos instead. That way, you can + // also detect revoked goals. + GOAL_YELLOW = 14 [deprecated = true]; + // The blue team just scored a goal. See also GOAL_YELLOW. + GOAL_BLUE = 15 [deprecated = true]; // Equivalent to STOP, but the yellow team must pick up the ball and // drop it in the Designated Position. BALL_PLACEMENT_YELLOW = 16; @@ -163,6 +167,12 @@ message Referee // Indicate if the team reached the maximum allowed ball placement failures and is // thus not allowed to place the ball anymore optional bool ball_placement_failures_reached = 15; + // The team is allowed to substitute one or more robots currently + optional bool bot_substitution_allowed = 16; + // The number of bot substitutions left by the team in this halftime + optional uint32 bot_substitutions_left = 17; + // The number of microseconds left for current bot substitution + optional uint32 bot_substitution_time_left = 18; } // Information about the two teams. @@ -185,6 +195,11 @@ message Referee // coordinate system. Obviously, the yellow team will play on the opposite half. optional bool blue_team_on_positive_half = 10; + // The game event that caused the referee command. + // deprecated in favor of game_events. + // optional Game_Event game_event = 11 [deprecated = true]; + reserved 11; + // The command that will be issued after the current stoppage and ball placement to // continue the game. optional Command next_command = 12; @@ -194,7 +209,7 @@ message Referee reserved 13; repeated GameEvent game_events = 16; - // All non-finished proposed game events that may be processed next. + // All proposed game events that were detected since the last RUNNING state. reserved 14; repeated GameEventProposalGroup game_event_proposals = 17; @@ -205,14 +220,19 @@ message Referee // * free kicks // * kickoff, penalty kick, force start // * ball placement - optional int32 current_action_time_remaining = 15; + optional int64 current_action_time_remaining = 15; + + // A message that can be displayed to the spectators, like a reason for a stoppage. + optional string status_message = 20; } // List of matching proposals message GameEventProposalGroup { - // The proposed game event. - repeated GameEvent game_event = 1; + // Unique ID of this group + optional string id = 3; + // The proposed game events + repeated GameEvent game_events = 1; // Whether the proposal group was accepted optional bool accepted = 2; } diff --git a/src/proto/ssl_gc_state.proto b/src/proto/ssl_gc_state.proto index 4cb9443aff..4ae750ff07 100644 --- a/src/proto/ssl_gc_state.proto +++ b/src/proto/ssl_gc_state.proto @@ -33,17 +33,17 @@ message Foul message Command { required Type type = 1; - optional Team for_team = 2; + required Team for_team = 2; enum Type { - UNKNOWN = 0; - HALT = 1; - STOP = 2; - NORMAL_START = 3; - FORCE_START = 4; - DIRECT = 5; - INDIRECT = 6; + UNKNOWN = 0; + HALT = 1; + STOP = 2; + NORMAL_START = 3; + FORCE_START = 4; + DIRECT = 5; + reserved 6; // INDIRECT KICKOFF = 7; PENALTY = 8; TIMEOUT = 9; @@ -80,10 +80,13 @@ message Proposal message ProposalGroup { - // List of proposals in this group + // Unique ID of this group + optional string id = 4; + // The proposals in this group repeated Proposal proposals = 1; // Whether the proposal group was accepted optional bool accepted = 2; + reserved 3; // uint32 id } message TeamInfo @@ -105,6 +108,9 @@ message TeamInfo optional google.protobuf.Timestamp requests_timeout_since = 15; optional google.protobuf.Timestamp requests_emergency_stop_since = 16; optional int32 challenge_flags = 17; + optional bool bot_substitution_allowed = 18; + optional int32 bot_substitutions_left = 19; + optional google.protobuf.Duration bot_substitution_time_left = 20; } message State @@ -122,7 +128,18 @@ message State repeated GameEvent game_events = 13; repeated ProposalGroup proposal_groups = 14; optional Division division = 15; - optional bool auto_continue = 16; - optional Team first_kickoff_team = 17; - optional MatchType match_type = 18; + reserved 16; + optional Team first_kickoff_team = 17; + optional MatchType match_type = 18; + optional google.protobuf.Timestamp ready_continue_time = 20; + optional ShootoutState shootout_state = 21; + optional string status_message = 22; + // The maximum number of bots per team (overwrites the division config) + optional int32 max_bots_per_team = 23; +} + +message ShootoutState +{ + optional Team next_team = 1; + map number_of_attempts = 2; } diff --git a/src/proto/tactic.proto b/src/proto/tactic.proto index ecb73872cd..afc537c1b4 100644 --- a/src/proto/tactic.proto +++ b/src/proto/tactic.proto @@ -28,7 +28,7 @@ message Tactic PivotKickTactic pivot_kick = 11; ReceiverTactic receiver = 12; ShadowEnemyTactic shadow_enemy = 13; - StopTactic stop = 14; + HaltTactic halt = 14; PassDefenderTactic pass_defender = 15; } } @@ -124,8 +124,7 @@ message MoveTactic required Point destination = 1; // The orientation the robot should have when it arrives at its destination required Angle final_orientation = 2; - // The speed the robot should have when it arrives at its destination - required double final_speed = 3; + reserved 3, 8; // How to run the dribbler required DribblerMode dribbler_mode = 4; // How to navigate around the ball @@ -134,8 +133,6 @@ message MoveTactic required AutoChipOrKick auto_chip_or_kick = 6; // The maximum allowed speed mode required MaxAllowedSpeedMode max_allowed_speed_mode = 7; - // The target spin while moving in revolutions per second - required double target_spin_rev_per_s = 8; // The obstacle avoidance mode to use while moving required ObstacleAvoidanceMode obstacle_avoidance_mode = 9; } @@ -179,4 +176,4 @@ message ShadowEnemyTactic required double shadow_distance = 2; } -message StopTactic {} +message HaltTactic {} diff --git a/src/shared/2021_robot_constants.cpp b/src/shared/2021_robot_constants.cpp index 303afdde6c..f2900737a4 100644 --- a/src/shared/2021_robot_constants.cpp +++ b/src/shared/2021_robot_constants.cpp @@ -17,8 +17,8 @@ RobotConstants_t create2021RobotConstants(void) .front_of_robot_width_meters = 0.11f, .dribbler_width_meters = 0.07825f, // Dribbler speeds are negative as that is the direction that sucks the ball in - .indefinite_dribbler_speed_rpm = -10000.0f, - .max_force_dribbler_speed_rpm = -12000.0f, + .indefinite_dribbler_speed_rpm = -10000, + .max_force_dribbler_speed_rpm = -12000, // Motor constant .motor_max_acceleration_m_per_s_2 = 4.5f, diff --git a/src/shared/BUILD b/src/shared/BUILD index 60261e870b..5178e4d7a1 100644 --- a/src/shared/BUILD +++ b/src/shared/BUILD @@ -5,20 +5,20 @@ load( "@platformio_rules//platformio:platformio.bzl", "platformio_library", ) +load("@bazel_skylib//rules:common_settings.bzl", "string_flag") + +platformio_library( + name = "constants_platformio", + hdr = "constants.h", +) cc_library( name = "constants", srcs = [], hdrs = ["constants.h"], - deps = [], alwayslink = True, ) -platformio_library( - name = "constants_platformio", - hdr = "constants.h", -) - cc_library( name = "robot_constants", srcs = [ @@ -28,7 +28,5 @@ cc_library( "2021_robot_constants.h", "robot_constants.h", ], - deps = [ - ":constants", - ], + deps = [":constants"], ) diff --git a/src/shared/constants.h b/src/shared/constants.h index a235bdfb3d..8ebff1acac 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -43,9 +43,10 @@ static const short unsigned int REDIS_DEFAULT_PORT = 6379; static const short unsigned int PRIMITIVE_PORT = 42070; // the port the AI receives msgs from the robot -static const short unsigned int ROBOT_STATUS_PORT = 42071; -static const short unsigned int ROBOT_LOGS_PORT = 42072; -static const short unsigned int ROBOT_CRASH_PORT = 42074; +static const short unsigned int ROBOT_STATUS_PORT = 42071; +static const short unsigned int ROBOT_LOGS_PORT = 42072; +static const short unsigned int ROBOT_CRASH_PORT = 42074; +static const short unsigned int NETWORK_COMM_TEST_PORT = 42075; // maximum transfer unit of the network interface // this is an int to avoid Wconversion with lwip diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index d8f283eb9b..af9bbee29e 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -67,11 +67,11 @@ typedef struct RobotConstants float dribbler_width_meters; // Indefinite dribbler mode sets a speed that can be maintained indefinitely [rpm] - float indefinite_dribbler_speed_rpm; + int indefinite_dribbler_speed_rpm; // Max force dribbler mode sets the speed that applies the maximum amount of force on // the ball [rpm] - float max_force_dribbler_speed_rpm; + int max_force_dribbler_speed_rpm; // The maximum acceleration achievable by our motors [m/s^2] float motor_max_acceleration_m_per_s_2; diff --git a/src/software/ai/hl/stp/play/BUILD b/src/software/ai/hl/stp/play/BUILD index d44cc02421..6074c57c7d 100644 --- a/src/software/ai/hl/stp/play/BUILD +++ b/src/software/ai/hl/stp/play/BUILD @@ -27,7 +27,7 @@ cc_library( deps = [ ":play", "//shared:constants", - "//software/ai/hl/stp/tactic/stop:stop_tactic", + "//software/ai/hl/stp/tactic/halt:halt_tactic", "//software/logger", "//software/util/generic_factory", ], @@ -82,9 +82,9 @@ cc_library( "//software/ai/hl/stp/tactic/attacker:attacker_tactic", "//software/ai/hl/stp/tactic/crease_defender:crease_defender_tactic", "//software/ai/hl/stp/tactic/goalie:goalie_tactic", + "//software/ai/hl/stp/tactic/halt:halt_tactic", "//software/ai/hl/stp/tactic/move:move_tactic", "//software/ai/hl/stp/tactic/shadow_enemy:shadow_enemy_tactic", - "//software/ai/hl/stp/tactic/stop:stop_tactic", "//software/logger", "//software/util/generic_factory", "//software/world:game_state", @@ -119,7 +119,7 @@ cc_library( deps = [ "//software/ai/hl/stp/tactic", "//software/ai/hl/stp/tactic/goalie:goalie_tactic", - "//software/ai/hl/stp/tactic/stop:stop_tactic", + "//software/ai/hl/stp/tactic/halt:halt_tactic", "//software/ai/motion_constraint:motion_constraint_set_builder", "//software/ai/navigator/trajectory:trajectory_planner", "//software/ai/passing:pass_with_rating", @@ -303,7 +303,7 @@ cc_library( deps = [ ":play", "//shared:constants", - "//software/ai/hl/stp/tactic/stop:stop_tactic", + "//software/ai/hl/stp/tactic/halt:halt_tactic", "//software/logger", "//software/util/generic_factory", ], diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp index f355ff3540..a8420ce8db 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.cpp @@ -56,10 +56,10 @@ void BallPlacementPlayFSM::alignPlacement(const Update &event) 2 * alignment_vector * ROBOT_MAX_RADIUS_METERS; align_placement_tactic->updateControlParams( - setup_point, setup_angle, 0.0, TbotsProto::DribblerMode::OFF, + setup_point, setup_angle, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + TbotsProto::ObstacleAvoidanceMode::SAFE); tactics_to_run[0].emplace_back(align_placement_tactic); @@ -143,10 +143,10 @@ void BallPlacementPlayFSM::retreat(const Update &event) // setup ball placement tactic for ball placing robot retreat_tactic->updateControlParams( - retreat_position, final_orientation, 0.0, TbotsProto::DribblerMode::OFF, + retreat_position, final_orientation, TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::AVOID, {AutoChipOrKickMode::OFF, 0}, TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + TbotsProto::ObstacleAvoidanceMode::SAFE); tactics_to_run[0].emplace_back(retreat_tactic); event.common.set_tactics(tactics_to_run); @@ -303,7 +303,7 @@ void BallPlacementPlayFSM::setupMoveTactics(const Update &event) waiting_line_vector.normalize(waiting_line_vector.length() * i / static_cast(move_tactics.size())); move_tactics.at(i)->updateControlParams( - waiting_destination, Angle::zero(), 0.0, + waiting_destination, Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::SAFE); } diff --git a/src/software/ai/hl/stp/play/enemy_ball_placement/enemy_ball_placement_play_fsm.cpp b/src/software/ai/hl/stp/play/enemy_ball_placement/enemy_ball_placement_play_fsm.cpp index 843d2de9b2..1412c30718 100644 --- a/src/software/ai/hl/stp/play/enemy_ball_placement/enemy_ball_placement_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/enemy_ball_placement/enemy_ball_placement_play_fsm.cpp @@ -95,13 +95,13 @@ void EnemyBallPlacementPlayFSM::avoid(const Update& event) } // Move to destination point while aligning to the ball avoid_interference_tactics[idx]->updateControlParams( - destination, (ball_pos - robot.position()).orientation(), 0); + destination, (ball_pos - robot.position()).orientation()); } else { // Stay in place while aligning to the ball avoid_interference_tactics[idx]->updateControlParams( - robot.position(), (ball_pos - robot.position()).orientation(), 0); + robot.position(), (ball_pos - robot.position()).orientation()); } tactics_to_run[0].emplace_back(avoid_interference_tactics[idx]); idx++; @@ -140,11 +140,10 @@ void EnemyBallPlacementPlayFSM::enterDefensiveFormation(const Update& event) Point right = world_ptr->ball().position() + right_vector; move_tactics[0]->updateControlParams( - center, positioning_vector.orientation() + Angle::half(), 0); - move_tactics[1]->updateControlParams(left, left_vector.orientation() + Angle::half(), - 0); + center, positioning_vector.orientation() + Angle::half()); + move_tactics[1]->updateControlParams(left, left_vector.orientation() + Angle::half()); move_tactics[2]->updateControlParams(right, - right_vector.orientation() + Angle::half(), 0); + right_vector.orientation() + Angle::half()); tactics_to_run[0].emplace_back(move_tactics[0]); tactics_to_run[0].emplace_back(move_tactics[1]); tactics_to_run[0].emplace_back(move_tactics[2]); diff --git a/src/software/ai/hl/stp/play/enemy_free_kick/BUILD b/src/software/ai/hl/stp/play/enemy_free_kick/BUILD index 061e8c5e84..552211c67d 100644 --- a/src/software/ai/hl/stp/play/enemy_free_kick/BUILD +++ b/src/software/ai/hl/stp/play/enemy_free_kick/BUILD @@ -24,9 +24,9 @@ cc_library( "//software/ai/hl/stp/play/defense:defense_play_base", "//software/ai/hl/stp/tactic/crease_defender:crease_defender_tactic", "//software/ai/hl/stp/tactic/goalie:goalie_tactic", + "//software/ai/hl/stp/tactic/halt:halt_tactic", "//software/ai/hl/stp/tactic/move:move_tactic", "//software/ai/hl/stp/tactic/pass_defender:pass_defender_tactic", - "//software/ai/hl/stp/tactic/stop:stop_tactic", "//software/logger", "//software/util/generic_factory", "//software/world:game_state", diff --git a/src/software/ai/hl/stp/play/example_play.cpp b/src/software/ai/hl/stp/play/example_play.cpp index 7cea6d0aaa..e9480f767b 100644 --- a/src/software/ai/hl/stp/play/example_play.cpp +++ b/src/software/ai/hl/stp/play/example_play.cpp @@ -25,7 +25,7 @@ void ExamplePlay::getNextTactics(TacticCoroutine::push_type &yield, world_ptr->ball().position() + Vector::createFromAngle(angle_between_robots * static_cast(k + 1)), - (angle_between_robots * static_cast(k + 1)) + Angle::half(), 0); + (angle_between_robots * static_cast(k + 1)) + Angle::half()); } // yield the Tactics this Play wants to run, in order of priority diff --git a/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp b/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp index 4df2039979..5ec0b9bced 100644 --- a/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/free_kick/free_kick_play_fsm.cpp @@ -69,7 +69,7 @@ void FreeKickPlayFSM::updateReceiverPositioningTactics( Angle receiver_orientation = (world->ball().position() - best_receiving_positions[i]).orientation(); receiver_positioning_tactics[i]->updateControlParams( - best_receiving_positions[i], receiver_orientation, 0.0, + best_receiving_positions[i], receiver_orientation, TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); } @@ -125,7 +125,7 @@ void FreeKickPlayFSM::updateAlignToBallTactic(const WorldPtr &world_ptr) align_to_ball_tactic->updateControlParams( ball_pos - direction_to_face.normalize(ROBOT_MAX_RADIUS_METERS * 2), - direction_to_face.orientation(), 0); + direction_to_face.orientation()); } bool FreeKickPlayFSM::shotFound(const Update &event) diff --git a/src/software/ai/hl/stp/play/halt_play.cpp b/src/software/ai/hl/stp/play/halt_play.cpp index 009332c535..0aa2e8ad87 100644 --- a/src/software/ai/hl/stp/play/halt_play.cpp +++ b/src/software/ai/hl/stp/play/halt_play.cpp @@ -1,6 +1,6 @@ #include "software/ai/hl/stp/play/halt_play.h" -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/util/generic_factory/generic_factory.h" HaltPlay::HaltPlay(TbotsProto::AiConfig config) : Play(config, false) {} @@ -8,18 +8,18 @@ HaltPlay::HaltPlay(TbotsProto::AiConfig config) : Play(config, false) {} void HaltPlay::getNextTactics(TacticCoroutine::push_type &yield, const WorldPtr &world_ptr) { - auto stop_tactic_1 = std::make_shared(); - auto stop_tactic_2 = std::make_shared(); - auto stop_tactic_3 = std::make_shared(); - auto stop_tactic_4 = std::make_shared(); - auto stop_tactic_5 = std::make_shared(); - auto stop_tactic_6 = std::make_shared(); + auto halt_tactic_1 = std::make_shared(); + auto halt_tactic_2 = std::make_shared(); + auto halt_tactic_3 = std::make_shared(); + auto halt_tactic_4 = std::make_shared(); + auto halt_tactic_5 = std::make_shared(); + auto halt_tactic_6 = std::make_shared(); do { // yield the Tactics this Play wants to run, in order of priority - yield({{stop_tactic_1, stop_tactic_2, stop_tactic_3, stop_tactic_4, stop_tactic_5, - stop_tactic_6}}); + yield({{halt_tactic_1, halt_tactic_2, halt_tactic_3, halt_tactic_4, halt_tactic_5, + halt_tactic_6}}); } while (true); } diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play.cpp index 1f0ff793cc..02e952e4fa 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/dribbling_parcour_play.cpp @@ -28,7 +28,7 @@ void DribblingParcourPlay::getNextTactics(TacticCoroutine::push_type &yield, } else { - move_tactic->updateControlParams(Point(0, 0), Angle::zero(), 0.0); + move_tactic->updateControlParams(Point(0, 0), Angle::zero()); result.emplace_back(move_tactic); } yield({result}); diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play.cpp index 9115c6cb9d..dce5722426 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/pass_endurance_play.cpp @@ -31,8 +31,7 @@ void PassEndurancePlay::getNextTactics(TacticCoroutine::push_type &yield, world_ptr->ball().position() + Vector::createFromAngle(angle_between_robots * static_cast(k + 1)), - (angle_between_robots * static_cast(k + 1)) + Angle::half(), - 0); + (angle_between_robots * static_cast(k + 1)) + Angle::half()); } } else @@ -44,7 +43,7 @@ void PassEndurancePlay::getNextTactics(TacticCoroutine::push_type &yield, auto next_position = Point( world_ptr->field().centerPoint().x(), (initial_offset + static_cast(k)) * 4 * ROBOT_MAX_RADIUS_METERS); - move_tactics[k]->updateControlParams(next_position, Angle::zero(), 0); + move_tactics[k]->updateControlParams(next_position, Angle::zero()); } } result.insert(result.end(), move_tactics.begin(), move_tactics.end()); diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play.cpp index f1d4c1c34e..fc021b7c2d 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_from_contested_possession_play.cpp @@ -30,7 +30,7 @@ void ScoringFromContestedPossessionPlay::getNextTactics(TacticCoroutine::push_ty else { // TODO (#2107): implement face ball opposite attacker 0.3m away - move_tactic->updateControlParams(Point(0, 0), Angle::zero(), 0.0); + move_tactic->updateControlParams(Point(0, 0), Angle::zero()); result.emplace_back(move_tactic); } yield({result}); diff --git a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play.cpp b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play.cpp index 27e5bca6d5..0d0d58ca7b 100644 --- a/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play.cpp +++ b/src/software/ai/hl/stp/play/hardware_challenge_plays/scoring_with_static_defenders_play.cpp @@ -29,7 +29,7 @@ void ScoringWithStaticDefendersPlay::getNextTactics(TacticCoroutine::push_type & auto next_position = Point( world_ptr->field().centerPoint().x(), (initial_offset + static_cast(k)) * 4 * ROBOT_MAX_RADIUS_METERS); - move_tactics[k]->updateControlParams(next_position, Angle::zero(), 0); + move_tactics[k]->updateControlParams(next_position, Angle::zero()); } } else if (world_ptr->gameState().isOurFreeKick()) @@ -46,8 +46,7 @@ void ScoringWithStaticDefendersPlay::getNextTactics(TacticCoroutine::push_type & world_ptr->ball().position() + Vector::createFromAngle(angle_between_robots * static_cast(k + 1)), - (angle_between_robots * static_cast(k + 1)) + Angle::half(), - 0); + (angle_between_robots * static_cast(k + 1)) + Angle::half()); } } result.insert(result.end(), move_tactics.begin(), move_tactics.end()); diff --git a/src/software/ai/hl/stp/play/kickoff_enemy_play.cpp b/src/software/ai/hl/stp/play/kickoff_enemy_play.cpp index d5cf51dd1f..694a0c6401 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy_play.cpp +++ b/src/software/ai/hl/stp/play/kickoff_enemy_play.cpp @@ -125,7 +125,7 @@ void KickoffEnemyPlay::getNextTactics(TacticCoroutine::push_type &yield, // listed above move_tactics.at(defense_position_index) ->updateControlParams(defense_positions.at(defense_position_index), - Angle::zero(), 0); + Angle::zero()); result[0].emplace_back(move_tactics.at(defense_position_index)); defense_position_index++; } @@ -138,7 +138,7 @@ void KickoffEnemyPlay::getNextTactics(TacticCoroutine::push_type &yield, world_ptr->field().friendlyGoalpostNeg(), world_ptr->field().centerPoint(), ROBOT_MAX_RADIUS_METERS), - Angle::zero(), 0, TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); result[0].emplace_back(move_tactics.at(defense_position_index)); diff --git a/src/software/ai/hl/stp/play/kickoff_friendly_play.cpp b/src/software/ai/hl/stp/play/kickoff_friendly_play.cpp index a5eabd312b..bec6ac1cc5 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly_play.cpp +++ b/src/software/ai/hl/stp/play/kickoff_friendly_play.cpp @@ -93,7 +93,7 @@ void KickoffFriendlyPlay::getNextTactics(TacticCoroutine::push_type &yield, for (unsigned i = 0; i < kickoff_setup_positions.size(); i++) { move_tactics.at(i)->updateControlParams(kickoff_setup_positions.at(i), - Angle::zero(), 0); + Angle::zero()); result[0].emplace_back(move_tactics.at(i)); } @@ -123,7 +123,7 @@ void KickoffFriendlyPlay::getNextTactics(TacticCoroutine::push_type &yield, for (unsigned i = 1; i < kickoff_setup_positions.size(); i++) { move_tactics.at(i)->updateControlParams(kickoff_setup_positions.at(i), - Angle::zero(), 0); + Angle::zero()); result[0].emplace_back(move_tactics.at(i)); } diff --git a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.cpp b/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.cpp index 2e47d85a32..e80017badd 100644 --- a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.cpp @@ -48,12 +48,11 @@ void PenaltyKickPlayFSM::setupPosition(const Update &event) ROBOT_MAX_RADIUS_METERS; penalty_setup_tactics.at(i)->updateControlParams( Point(ball_position_x - 1.25, y_offset), - event.common.world_ptr->field().enemyGoalCenter().toVector().orientation(), - 0); + event.common.world_ptr->field().enemyGoalCenter().toVector().orientation()); } // Move shooting robot behind the ball - penalty_setup_tactics.back()->updateControlParams(behind_ball, shoot_angle, 0.0); + penalty_setup_tactics.back()->updateControlParams(behind_ball, shoot_angle); tactics_to_run[0].insert(tactics_to_run[0].end(), penalty_setup_tactics.begin(), penalty_setup_tactics.end()); diff --git a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.h b/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.h index d6733c55b6..5fe8a5481c 100644 --- a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.h +++ b/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_fsm.h @@ -3,9 +3,9 @@ #include "proto/parameters.pb.h" #include "shared/constants.h" #include "software/ai/hl/stp/play/play_fsm.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/ai/hl/stp/tactic/move/move_tactic.h" #include "software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic.h" -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" #include "software/logger/logger.h" diff --git a/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_fsm.cpp b/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_fsm.cpp index 7f7a3600e5..2f8dd591fb 100644 --- a/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_fsm.cpp @@ -26,8 +26,7 @@ void PenaltyKickEnemyPlayFSM::setupPosition(const Update &event) move_tactics.at(i)->updateControlParams( Point(event.common.world_ptr->field().enemyPenaltyMark().x() + 1.75, y_offset), - event.common.world_ptr->field().enemyGoalCenter().toVector().orientation(), - 0); + event.common.world_ptr->field().enemyGoalCenter().toVector().orientation()); } // Move goalie to the goal line diff --git a/src/software/ai/hl/stp/play/play.cpp b/src/software/ai/hl/stp/play/play.cpp index 85cc5755be..dd317da133 100644 --- a/src/software/ai/hl/stp/play/play.cpp +++ b/src/software/ai/hl/stp/play/play.cpp @@ -5,7 +5,7 @@ #include #include "proto/message_translation/tbots_protobuf.h" -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/ai/motion_constraint/motion_constraint_set_builder.h" #include "software/logger/logger.h" @@ -13,7 +13,7 @@ Play::Play(TbotsProto::AiConfig ai_config, bool requires_goalie) : ai_config(ai_config), goalie_tactic(std::make_shared(ai_config)), - stop_tactics(), + halt_tactics(), requires_goalie(requires_goalie), tactic_sequence(boost::bind(&Play::getNextTacticsWrapper, this, _1)), world_ptr_(std::nullopt), @@ -21,7 +21,7 @@ Play::Play(TbotsProto::AiConfig ai_config, bool requires_goalie) { for (unsigned int i = 0; i < MAX_ROBOT_IDS; i++) { - stop_tactics.push_back(std::make_shared()); + halt_tactics.push_back(std::make_shared()); } } @@ -189,10 +189,10 @@ std::unique_ptr Play::get( else if (i == (priority_tactics.size() - 1)) { // If assigning the last tactic vector, then assign rest of robots with - // StopTactics + // HaltTactics for (unsigned int ii = 0; ii < (robots.size() - num_tactics); ii++) { - tactic_vector.push_back(stop_tactics[ii]); + tactic_vector.push_back(halt_tactics[ii]); } } diff --git a/src/software/ai/hl/stp/play/play.h b/src/software/ai/hl/stp/play/play.h index 1a87f6934c..cad46f0206 100644 --- a/src/software/ai/hl/stp/play/play.h +++ b/src/software/ai/hl/stp/play/play.h @@ -170,8 +170,8 @@ class Play virtual void getNextTactics(TacticCoroutine::push_type& yield, const WorldPtr& world_ptr) = 0; - // Stop tactic common to all plays for robots that don't have tactics assigned - TacticVector stop_tactics; + // HaltTactics common to all plays for robots that don't have tactics assigned + TacticVector halt_tactics; // Whether this play requires a goalie const bool requires_goalie; diff --git a/src/software/ai/hl/stp/play/shoot_or_chip_play.cpp b/src/software/ai/hl/stp/play/shoot_or_chip_play.cpp index 8a34000f60..891574bff4 100644 --- a/src/software/ai/hl/stp/play/shoot_or_chip_play.cpp +++ b/src/software/ai/hl/stp/play/shoot_or_chip_play.cpp @@ -7,9 +7,9 @@ #include "software/ai/evaluation/possession.h" #include "software/ai/hl/stp/tactic/attacker/attacker_tactic.h" #include "software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/ai/hl/stp/tactic/move/move_tactic.h" #include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.h" -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" #include "software/logger/logger.h" #include "software/util/generic_factory/generic_factory.h" #include "software/world/game_state.h" @@ -79,7 +79,7 @@ void ShootOrChipPlay::getNextTactics(TacticCoroutine::push_type &yield, chip_targets[i].origin() - Vector::createFromAngle(orientation).normalize(ROBOT_MAX_RADIUS_METERS); ; - move_to_open_area_tactics[i]->updateControlParams(position, orientation, 0.0); + move_to_open_area_tactics[i]->updateControlParams(position, orientation); result[0].emplace_back(move_to_open_area_tactics[i]); } diff --git a/src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_fsm.cpp b/src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_fsm.cpp index 04142dd963..a68cdbd33e 100644 --- a/src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_fsm.cpp @@ -50,7 +50,7 @@ void ShootOrPassPlayFSM::updateOffensivePositioningTactics( Angle receiver_orientation = (world->ball().position() - best_receiving_positions[i]).orientation(); offensive_positioning_tactics[i]->updateControlParams( - best_receiving_positions[i], receiver_orientation, 0.0, + best_receiving_positions[i], receiver_orientation, TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); } diff --git a/src/software/ai/hl/stp/play/stop_play.cpp b/src/software/ai/hl/stp/play/stop_play.cpp index 24d5a1223c..4488bfea79 100644 --- a/src/software/ai/hl/stp/play/stop_play.cpp +++ b/src/software/ai/hl/stp/play/stop_play.cpp @@ -80,15 +80,15 @@ void StopPlay::getNextTactics(TacticCoroutine::push_type &yield, move_tactics.at(0)->updateControlParams( ball_defense_point_center, - (world_ptr->ball().position() - ball_defense_point_center).orientation(), 0, + (world_ptr->ball().position() - ball_defense_point_center).orientation(), stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); move_tactics.at(1)->updateControlParams( ball_defense_point_left, - (world_ptr->ball().position() - ball_defense_point_left).orientation(), 0, + (world_ptr->ball().position() - ball_defense_point_left).orientation(), stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); move_tactics.at(2)->updateControlParams( ball_defense_point_right, - (world_ptr->ball().position() - ball_defense_point_right).orientation(), 0, + (world_ptr->ball().position() - ball_defense_point_right).orientation(), stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); std::get<0>(crease_defender_tactics) diff --git a/src/software/ai/hl/stp/play/test_plays/BUILD b/src/software/ai/hl/stp/play/test_plays/BUILD index 5ddb293225..bc08c72b17 100644 --- a/src/software/ai/hl/stp/play/test_plays/BUILD +++ b/src/software/ai/hl/stp/play/test_plays/BUILD @@ -6,7 +6,7 @@ cc_library( hdrs = ["halt_test_play.h"], deps = [ "//software/ai/hl/stp/play", - "//software/ai/hl/stp/tactic/stop:stop_tactic", + "//software/ai/hl/stp/tactic/halt:halt_tactic", "//software/util/generic_factory", ], alwayslink = True, diff --git a/src/software/ai/hl/stp/play/test_plays/halt_test_play.cpp b/src/software/ai/hl/stp/play/test_plays/halt_test_play.cpp index f670f96fb0..95b84e9af0 100644 --- a/src/software/ai/hl/stp/play/test_plays/halt_test_play.cpp +++ b/src/software/ai/hl/stp/play/test_plays/halt_test_play.cpp @@ -1,6 +1,6 @@ #include "software/ai/hl/stp/play/test_plays/halt_test_play.h" -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/geom/algorithms/contains.h" #include "software/util/generic_factory/generic_factory.h" @@ -9,13 +9,13 @@ HaltTestPlay::HaltTestPlay(TbotsProto::AiConfig config) : Play(config, false) {} void HaltTestPlay::getNextTactics(TacticCoroutine::push_type &yield, const WorldPtr &world_ptr) { - auto stop_test_tactic_1 = std::make_shared(); - auto stop_test_tactic_2 = std::make_shared(); - auto stop_test_tactic_3 = std::make_shared(); + auto halt_test_tactic_1 = std::make_shared(); + auto halt_test_tactic_2 = std::make_shared(); + auto halt_test_tactic_3 = std::make_shared(); do { - yield({{stop_test_tactic_1, stop_test_tactic_2, stop_test_tactic_3}}); + yield({{halt_test_tactic_1, halt_test_tactic_2, halt_test_tactic_3}}); } while (true); } diff --git a/src/software/ai/hl/stp/play/test_plays/move_test_play.cpp b/src/software/ai/hl/stp/play/test_plays/move_test_play.cpp index bd25e91ce3..6204481d78 100644 --- a/src/software/ai/hl/stp/play/test_plays/move_test_play.cpp +++ b/src/software/ai/hl/stp/play/test_plays/move_test_play.cpp @@ -15,16 +15,15 @@ void MoveTestPlay::getNextTactics(TacticCoroutine::push_type &yield, do { move_test_tactic_friendly_goal->updateControlParams( - world_ptr->field().friendlyGoalCenter(), Angle::zero(), 0, + world_ptr->field().friendlyGoalCenter(), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::SAFE); move_test_tactic_enemy_goal->updateControlParams( - world_ptr->field().enemyGoalCenter(), Angle::zero(), 0, + world_ptr->field().enemyGoalCenter(), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::SAFE); move_test_tactic_center_field->updateControlParams( - Point(0, 0), Angle::zero(), 0, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + Point(0, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::SAFE); yield({{move_test_tactic_center_field, move_test_tactic_friendly_goal, diff --git a/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp b/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp index e4856b67cc..2fc875b098 100644 --- a/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp +++ b/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp @@ -6,7 +6,7 @@ #include "software/ai/hl/stp/play/halt_play.h" #include "software/ai/hl/stp/stp.h" #include "software/ai/hl/stp/tactic/all_tactics.h" -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/test_util/test_util.h" /** @@ -72,9 +72,9 @@ TEST_F(STPTacticAssignmentTest, auto move_tactic_1 = std::make_shared(); auto move_tactic_2 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), 0, + move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1, move_tactic_2}; @@ -96,9 +96,9 @@ TEST_F(STPTacticAssignmentTest, auto move_tactic_1 = std::make_shared(); auto move_tactic_2 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), 0, + move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1, move_tactic_2}; @@ -122,7 +122,7 @@ TEST_F(STPTacticAssignmentTest, auto move_tactic_1 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1}; @@ -140,7 +140,7 @@ TEST_F(STPTacticAssignmentTest, test_0_tactics_returned_when_there_are_no_robots auto move_tactic_1 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1}; @@ -160,12 +160,12 @@ TEST_F(STPTacticAssignmentTest, world.updateFriendlyTeamState(friendly_team); auto move_tactic_1 = std::make_shared(); - auto stop_tactic_1 = std::make_shared(); + auto halt_tactic_1 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - ConstTacticVector tactics = {move_tactic_1, stop_tactic_1}; + ConstTacticVector tactics = {move_tactic_1, halt_tactic_1}; // Both robots are now closest to move_tactic_1's destination. We do NOT want // robot_0 to be assigned to move_tactic_1, because then robot_1 has to move all the @@ -175,11 +175,11 @@ TEST_F(STPTacticAssignmentTest, auto asst = stp.assignRobotsToTactics({tactics}, world, false); - // move_tactic_1 should be the only Tactic assigned a robot, since stop_tactic_1 is a + // move_tactic_1 should be the only Tactic assigned a robot, since halt_tactic_1 is a // lower priority than move_tactic_1 so it should be dropped since there's only 1 // robot EXPECT_TRUE(asst.find(move_tactic_1) != asst.end()); - EXPECT_FALSE(asst.find(stop_tactic_1) != asst.end()); + EXPECT_FALSE(asst.find(halt_tactic_1) != asst.end()); } @@ -193,7 +193,7 @@ TEST_F(STPTacticAssignmentTest, test_assigning_1_tactic_to_1_robot) auto move_tactic_1 = std::make_shared(); - move_tactic_1->updateControlParams(Point(2, -3.2), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(2, -3.2), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1}; @@ -221,9 +221,9 @@ TEST_F(STPTacticAssignmentTest, test_assigning_2_robots_to_2_tactics_no_overlap) auto move_tactic_1 = std::make_shared(); auto move_tactic_2 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), 0, + move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1, move_tactic_2}; @@ -263,9 +263,9 @@ TEST_F(STPTacticAssignmentTest, test_assigning_2_robots_to_2_tactics_with_overla auto move_tactic_1 = std::make_shared(); auto move_tactic_2 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), 0, + move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1, move_tactic_2}; @@ -298,9 +298,9 @@ TEST_F(STPTacticAssignmentTest, test_assigning_3_robots_to_2_tactics) auto move_tactic_1 = std::make_shared(); auto move_tactic_2 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), 0, + move_tactic_2->updateControlParams(Point(1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1, move_tactic_2}; @@ -329,21 +329,21 @@ TEST_F(STPTacticAssignmentTest, friendly_team.updateRobots({robot_0, robot_1, robot_2}); world.updateFriendlyTeamState(friendly_team); - auto stop_tactic_1 = std::make_shared(); - auto stop_tactic_2 = std::make_shared(); - auto stop_tactic_3 = std::make_shared(); + auto halt_tactic_1 = std::make_shared(); + auto halt_tactic_2 = std::make_shared(); + auto halt_tactic_3 = std::make_shared(); - ConstTacticVector tactics = {stop_tactic_1, stop_tactic_2, stop_tactic_3}; + ConstTacticVector tactics = {halt_tactic_1, halt_tactic_2, halt_tactic_3}; // If all costs are equal, the robots and tactics are simply paired in order auto asst = stp.assignRobotsToTactics({tactics}, world, false); - ASSERT_TRUE(asst.find(stop_tactic_1) != asst.end()); - ASSERT_TRUE(asst.find(stop_tactic_2) != asst.end()); - ASSERT_TRUE(asst.find(stop_tactic_3) != asst.end()); - EXPECT_EQ(asst.find(stop_tactic_1)->second, robot_0); - EXPECT_EQ(asst.find(stop_tactic_2)->second, robot_1); - EXPECT_EQ(asst.find(stop_tactic_3)->second, robot_2); + ASSERT_TRUE(asst.find(halt_tactic_1) != asst.end()); + ASSERT_TRUE(asst.find(halt_tactic_2) != asst.end()); + ASSERT_TRUE(asst.find(halt_tactic_3) != asst.end()); + EXPECT_EQ(asst.find(halt_tactic_1)->second, robot_0); + EXPECT_EQ(asst.find(halt_tactic_2)->second, robot_1); + EXPECT_EQ(asst.find(halt_tactic_3)->second, robot_2); } TEST_F(STPTacticAssignmentTest, @@ -359,26 +359,26 @@ TEST_F(STPTacticAssignmentTest, friendly_team.updateRobots({robot_0, robot_1, robot_2}); world.updateFriendlyTeamState(friendly_team); - auto stop_tactic_1 = std::make_shared(); + auto halt_tactic_1 = std::make_shared(); auto move_tactic_1 = std::make_shared(); - auto stop_tactic_2 = std::make_shared(); + auto halt_tactic_2 = std::make_shared(); // The destination of the move_tactic is relatively close to the robot positions, so // the cost of assigning any robot to the move_tactic should be less than the - // stop_tactics - move_tactic_1->updateControlParams(Point(0, 0), Angle::zero(), 0, + // halt_tactics + move_tactic_1->updateControlParams(Point(0, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - ConstTacticVector tactics = {stop_tactic_1, move_tactic_1, stop_tactic_2}; + ConstTacticVector tactics = {halt_tactic_1, move_tactic_1, halt_tactic_2}; auto asst = stp.assignRobotsToTactics({tactics}, world, false); - ASSERT_TRUE(asst.find(stop_tactic_1) != asst.end()); + ASSERT_TRUE(asst.find(halt_tactic_1) != asst.end()); ASSERT_TRUE(asst.find(move_tactic_1) != asst.end()); - ASSERT_TRUE(asst.find(stop_tactic_2) != asst.end()); - EXPECT_EQ(asst.find(stop_tactic_1)->second, robot_2); + ASSERT_TRUE(asst.find(halt_tactic_2) != asst.end()); + EXPECT_EQ(asst.find(halt_tactic_1)->second, robot_2); EXPECT_EQ(asst.find(move_tactic_1)->second, robot_0); - EXPECT_EQ(asst.find(stop_tactic_2)->second, robot_1); + EXPECT_EQ(asst.find(halt_tactic_2)->second, robot_1); } TEST_F(STPTacticAssignmentTest, @@ -397,7 +397,7 @@ TEST_F(STPTacticAssignmentTest, auto move_tactic_1 = std::make_shared(); - move_tactic_1->updateControlParams(Point(0, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(0, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1}; @@ -450,9 +450,9 @@ TEST_F(STPTacticAssignmentTest, } TEST_F(STPTacticAssignmentTest, - test_assigning_stop_tactics_to_unassigned_non_goalie_robots) + test_assigning_halt_tactics_to_unassigned_non_goalie_robots) { - // Test that StopTactic is assigned to remaining robots without tactics + // Test that HaltTactic is assigned to remaining robots without tactics Team friendly_team(Duration::fromSeconds(0)); Robot robot_0(0, Point(-1, 1), Vector(), Angle::zero(), AngularVelocity::zero(), @@ -466,7 +466,7 @@ TEST_F(STPTacticAssignmentTest, auto move_tactic_1 = std::make_shared(); - move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(-1, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstTacticVector tactics = {move_tactic_1}; @@ -513,9 +513,9 @@ TEST_F(STPTacticAssignmentTest, test_greediness_of_tiered_assignment) auto move_tactic_0 = std::make_shared(); auto move_tactic_1 = std::make_shared(); - move_tactic_0->updateControlParams(Point(0, 0), Angle::zero(), 0, + move_tactic_0->updateControlParams(Point(0, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); - move_tactic_1->updateControlParams(Point(2, 0), Angle::zero(), 0, + move_tactic_1->updateControlParams(Point(2, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); ConstPriorityTacticVector normal_tactics = {{move_tactic_0, move_tactic_1}}; diff --git a/src/software/ai/hl/stp/stp_test.cpp b/src/software/ai/hl/stp/stp_test.cpp index 4026521735..0cdf72e706 100644 --- a/src/software/ai/hl/stp/stp_test.cpp +++ b/src/software/ai/hl/stp/stp_test.cpp @@ -42,7 +42,7 @@ TEST_F(STPTest, test_get_play_info) std::string expected_play_name, expected_tactic_name; expected_play_name = "HaltPlay"; - expected_tactic_name = "StopTactic"; + expected_tactic_name = "HaltTactic"; TbotsProto::PlayInfo expected_play_info_msg = TbotsProto::PlayInfo(); expected_play_info_msg.mutable_play()->set_play_name(expected_play_name); diff --git a/src/software/ai/hl/stp/tactic/BUILD b/src/software/ai/hl/stp/tactic/BUILD index 5baefe0328..505da40cd1 100644 --- a/src/software/ai/hl/stp/tactic/BUILD +++ b/src/software/ai/hl/stp/tactic/BUILD @@ -21,6 +21,7 @@ cc_library( "//software/ai/hl/stp/tactic/dribble:dribble_tactic", "//software/ai/hl/stp/tactic/get_behind_ball:get_behind_ball_tactic", "//software/ai/hl/stp/tactic/goalie:goalie_tactic", + "//software/ai/hl/stp/tactic/halt:halt_tactic", "//software/ai/hl/stp/tactic/kick:kick_tactic", "//software/ai/hl/stp/tactic/move:move_tactic", "//software/ai/hl/stp/tactic/pass_defender:pass_defender_tactic", @@ -28,7 +29,6 @@ cc_library( "//software/ai/hl/stp/tactic/pivot_kick:pivot_kick_tactic", "//software/ai/hl/stp/tactic/receiver:receiver_tactic", "//software/ai/hl/stp/tactic/shadow_enemy:shadow_enemy_tactic", - "//software/ai/hl/stp/tactic/stop:stop_tactic", ], ) diff --git a/src/software/ai/hl/stp/tactic/all_tactics.h b/src/software/ai/hl/stp/tactic/all_tactics.h index 46b0df0f0c..9ca1d50a4e 100644 --- a/src/software/ai/hl/stp/tactic/all_tactics.h +++ b/src/software/ai/hl/stp/tactic/all_tactics.h @@ -6,6 +6,7 @@ #include "software/ai/hl/stp/tactic/dribble/dribble_tactic.h" #include "software/ai/hl/stp/tactic/get_behind_ball/get_behind_ball_tactic.h" #include "software/ai/hl/stp/tactic/goalie/goalie_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include "software/ai/hl/stp/tactic/kick/kick_tactic.h" #include "software/ai/hl/stp/tactic/move/move_tactic.h" #include "software/ai/hl/stp/tactic/pass_defender/pass_defender_tactic.h" @@ -13,5 +14,4 @@ #include "software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h" #include "software/ai/hl/stp/tactic/receiver/receiver_tactic.h" #include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.h" -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" #include "software/ai/hl/stp/tactic/tactic.h" diff --git a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp b/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp index f0ccaa33d3..1a4239c715 100644 --- a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_fsm.cpp @@ -144,13 +144,11 @@ void CreaseDefenderFSM::blockThreat( MoveFSM::ControlParams control_params{ .destination = destination, .final_orientation = face_threat_orientation, - .final_speed = 0.0, .dribbler_mode = TbotsProto::DribblerMode::OFF, .ball_collision_type = ball_collision_type, .auto_chip_or_kick = auto_chip_or_kick, .max_allowed_speed_mode = event.control_params.max_allowed_speed_mode, - .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, - .target_spin_rev_per_s = 0.0}; + .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE}; // Update the get behind ball fsm processEvent(MoveFSM::Update(control_params, event.common)); diff --git a/src/software/ai/hl/stp/tactic/stop/BUILD b/src/software/ai/hl/stp/tactic/halt/BUILD similarity index 68% rename from src/software/ai/hl/stp/tactic/stop/BUILD rename to src/software/ai/hl/stp/tactic/halt/BUILD index d9a730d949..266edf3f11 100644 --- a/src/software/ai/hl/stp/tactic/stop/BUILD +++ b/src/software/ai/hl/stp/tactic/halt/BUILD @@ -1,14 +1,14 @@ package(default_visibility = ["//visibility:public"]) cc_library( - name = "stop_tactic", + name = "halt_tactic", srcs = [ - "stop_fsm.cpp", - "stop_tactic.cpp", + "halt_fsm.cpp", + "halt_tactic.cpp", ], hdrs = [ - "stop_fsm.h", - "stop_tactic.h", + "halt_fsm.h", + "halt_tactic.h", ], deps = [ "//shared:constants", @@ -18,20 +18,20 @@ cc_library( ) cc_test( - name = "stop_fsm_test", - srcs = ["stop_fsm_test.cpp"], + name = "halt_fsm_test", + srcs = ["halt_fsm_test.cpp"], deps = [ - ":stop_tactic", + ":halt_tactic", "//shared/test_util:tbots_gtest_main", "//software/test_util", ], ) cc_test( - name = "stop_tactic_test", - srcs = ["stop_tactic_test.cpp"], + name = "halt_tactic_test", + srcs = ["halt_tactic_test.cpp"], deps = [ - ":stop_tactic", + ":halt_tactic", "//shared/test_util:tbots_gtest_main", "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", "//software/simulated_tests/terminating_validation_functions", diff --git a/src/software/ai/hl/stp/tactic/halt/halt_fsm.cpp b/src/software/ai/hl/stp/tactic/halt/halt_fsm.cpp new file mode 100644 index 0000000000..c8ca316eb3 --- /dev/null +++ b/src/software/ai/hl/stp/tactic/halt/halt_fsm.cpp @@ -0,0 +1,11 @@ +#include "software/ai/hl/stp/tactic/halt/halt_fsm.h" + +void HaltFSM::updateStop(const Update& event) +{ + event.common.set_primitive(std::make_unique()); +} + +bool HaltFSM::stopDone(const Update& event) +{ + return robotStopped(event.common.robot); +} diff --git a/src/software/ai/hl/stp/tactic/stop/stop_fsm.h b/src/software/ai/hl/stp/tactic/halt/halt_fsm.h similarity index 81% rename from src/software/ai/hl/stp/tactic/stop/stop_fsm.h rename to src/software/ai/hl/stp/tactic/halt/halt_fsm.h index d5c35ad38d..dae78c1112 100644 --- a/src/software/ai/hl/stp/tactic/stop/stop_fsm.h +++ b/src/software/ai/hl/stp/tactic/halt/halt_fsm.h @@ -2,7 +2,7 @@ #include "software/ai/hl/stp/tactic/tactic.h" -struct StopFSM +struct HaltFSM { public: class StopState; @@ -14,23 +14,23 @@ struct StopFSM DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS /** - * Constructor for StopFSM struct + * Constructor for HaltFSM struct */ - explicit StopFSM() {} + explicit HaltFSM() {} /** * Action to set the StopPrimitive * - * @param event StopFSM::Update + * @param event HaltFSM::Update */ void updateStop(const Update& event); /** - * Guard if the stop is done + * Guard if the halt is done * - * @param event StopFSM::Update + * @param event HaltFSM::Update * - * @return if the robot has stopped + * @return if the robot has halted */ bool stopDone(const Update& event); diff --git a/src/software/ai/hl/stp/tactic/stop/stop_fsm_test.cpp b/src/software/ai/hl/stp/tactic/halt/halt_fsm_test.cpp similarity index 71% rename from src/software/ai/hl/stp/tactic/stop/stop_fsm_test.cpp rename to src/software/ai/hl/stp/tactic/halt/halt_fsm_test.cpp index d4672ab7ae..849ac30428 100644 --- a/src/software/ai/hl/stp/tactic/stop/stop_fsm_test.cpp +++ b/src/software/ai/hl/stp/tactic/halt/halt_fsm_test.cpp @@ -1,4 +1,4 @@ -#include "software/ai/hl/stp/tactic/stop/stop_fsm.h" +#include "software/ai/hl/stp/tactic/halt/halt_fsm.h" #include @@ -12,22 +12,22 @@ TEST(StopFSMTest, test_transitions) AngularVelocity::zero()), Timestamp::fromSeconds(123)); - FSM fsm{StopFSM()}; - EXPECT_TRUE(fsm.is(boost::sml::state)); - fsm.process_event(StopFSM::Update( + FSM fsm{HaltFSM()}; + EXPECT_TRUE(fsm.is(boost::sml::state)); + fsm.process_event(HaltFSM::Update( {}, TacticUpdate(robot, world, [](std::shared_ptr) {}))); // robot is still moving - EXPECT_TRUE(fsm.is(boost::sml::state)); + EXPECT_TRUE(fsm.is(boost::sml::state)); robot = Robot(0, RobotState(Point(1, -3), Vector(1.1, 2.1), Angle::half(), AngularVelocity::zero()), Timestamp::fromSeconds(123)); - fsm.process_event(StopFSM::Update( + fsm.process_event(HaltFSM::Update( {}, TacticUpdate(robot, world, [](std::shared_ptr) {}))); // robot is still moving - EXPECT_TRUE(fsm.is(boost::sml::state)); + EXPECT_TRUE(fsm.is(boost::sml::state)); robot = TestUtil::createRobotAtPos(Point(1, -3)); - fsm.process_event(StopFSM::Update( + fsm.process_event(HaltFSM::Update( {}, TacticUpdate(robot, world, [](std::shared_ptr) {}))); // robot stopped EXPECT_TRUE(fsm.is(boost::sml::X)); diff --git a/src/software/ai/hl/stp/tactic/halt/halt_tactic.cpp b/src/software/ai/hl/stp/tactic/halt/halt_tactic.cpp new file mode 100644 index 0000000000..49e6e1fe01 --- /dev/null +++ b/src/software/ai/hl/stp/tactic/halt/halt_tactic.cpp @@ -0,0 +1,26 @@ +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" + +#include + +HaltTactic::HaltTactic() : Tactic(std::set()), fsm_map() +{ + for (RobotId id = 0; id < MAX_ROBOT_IDS; id++) + { + fsm_map[id] = std::make_unique>(HaltFSM()); + } +} + +void HaltTactic::accept(TacticVisitor &visitor) const +{ + visitor.visit(*this); +} + +void HaltTactic::updatePrimitive(const TacticUpdate &tactic_update, bool reset_fsm) +{ + if (reset_fsm) + { + fsm_map[tactic_update.robot.id()] = std::make_unique>(HaltFSM()); + } + fsm_map.at(tactic_update.robot.id()) + ->process_event(HaltFSM::Update({}, tactic_update)); +} diff --git a/src/software/ai/hl/stp/tactic/stop/stop_tactic.h b/src/software/ai/hl/stp/tactic/halt/halt_tactic.h similarity index 56% rename from src/software/ai/hl/stp/tactic/stop/stop_tactic.h rename to src/software/ai/hl/stp/tactic/halt/halt_tactic.h index 8cc9486b8d..39bce73916 100644 --- a/src/software/ai/hl/stp/tactic/stop/stop_tactic.h +++ b/src/software/ai/hl/stp/tactic/halt/halt_tactic.h @@ -1,19 +1,19 @@ #pragma once -#include "software/ai/hl/stp/tactic/stop/stop_fsm.h" +#include "software/ai/hl/stp/tactic/halt/halt_fsm.h" #include "software/ai/hl/stp/tactic/tactic.h" /** - * The StopTactic will stop the robot from moving. The robot will actively try and brake + * The HaltTactic will stop the robot from moving. The robot will actively try and brake * to come to a halt. */ -class StopTactic : public Tactic +class HaltTactic : public Tactic { public: /** - * Creates a new StopTactic + * Creates a new HaltTactic */ - explicit StopTactic(); + explicit HaltTactic(); void accept(TacticVisitor& visitor) const override; @@ -22,5 +22,5 @@ class StopTactic : public Tactic private: void updatePrimitive(const TacticUpdate& tactic_update, bool reset_fsm) override; - std::map>> fsm_map; + std::map>> fsm_map; }; diff --git a/src/software/ai/hl/stp/tactic/stop/stop_tactic_test.cpp b/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp similarity index 88% rename from src/software/ai/hl/stp/tactic/stop/stop_tactic_test.cpp rename to src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp index 0164465d98..e2867bae07 100644 --- a/src/software/ai/hl/stp/tactic/stop/stop_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp @@ -1,4 +1,4 @@ -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" +#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" #include @@ -8,14 +8,14 @@ #include "software/simulated_tests/validation/validation_function.h" #include "software/test_util/test_util.h" -class StopTacticTest : public SimulatedErForceSimPlayTestFixture +class HaltTacticTest : public SimulatedErForceSimPlayTestFixture { protected: TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; Field field = Field::createField(field_type); }; -TEST_F(StopTacticTest, robot_already_stopped) +TEST_F(HaltTacticTest, robot_already_stopped) { BallState ball_state(Point(0, 0.5), Vector(0, 0)); @@ -23,7 +23,7 @@ TEST_F(StopTacticTest, robot_already_stopped) {Point(-3, 2.5), Point()}, {Vector(), Vector()}); auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - auto tactic = std::make_shared(); + auto tactic = std::make_shared(); setTactic(1, tactic); std::vector terminating_validation_functions = { @@ -41,7 +41,7 @@ TEST_F(StopTacticTest, robot_already_stopped) Duration::fromSeconds(5)); } -TEST_F(StopTacticTest, robot_start_moving) +TEST_F(HaltTacticTest, robot_start_moving) { BallState ball_state(Point(0, 0.5), Vector(0, 0)); @@ -49,7 +49,7 @@ TEST_F(StopTacticTest, robot_start_moving) {Point(-3, 2.5), Point()}, {Vector(), Vector(4, 4)}); auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - auto tactic = std::make_shared(); + auto tactic = std::make_shared(); setTactic(1, tactic); std::vector terminating_validation_functions = { diff --git a/src/software/ai/hl/stp/tactic/move/move_fsm.h b/src/software/ai/hl/stp/tactic/move/move_fsm.h index 936d7d8958..34fd11e180 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm.h +++ b/src/software/ai/hl/stp/tactic/move/move_fsm.h @@ -17,8 +17,6 @@ struct MoveFSM Point destination; // The orientation the robot should have when it arrives at its destination Angle final_orientation; - // The speed the robot should have when it arrives at its destination - double final_speed; // How to run the dribbler TbotsProto::DribblerMode dribbler_mode; // How to navigate around the ball @@ -29,8 +27,6 @@ struct MoveFSM TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode; // The obstacle avoidance mode TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode; - // The target spin while moving in revolutions per second - double target_spin_rev_per_s; }; // this struct defines the only event that the MoveFSM responds to diff --git a/src/software/ai/hl/stp/tactic/move/move_fsm_test.cpp b/src/software/ai/hl/stp/tactic/move/move_fsm_test.cpp index f242672d50..73448ce6a2 100644 --- a/src/software/ai/hl/stp/tactic/move/move_fsm_test.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_fsm_test.cpp @@ -11,13 +11,11 @@ TEST(MoveFSMTest, test_transitions) MoveFSM::ControlParams control_params{ .destination = Point(2, 3), .final_orientation = Angle::half(), - .final_speed = 0.0, .dribbler_mode = TbotsProto::DribblerMode::OFF, .ball_collision_type = TbotsProto::BallCollisionType::AVOID, .auto_chip_or_kick = AutoChipOrKick{AutoChipOrKickMode::OFF, 0}, .max_allowed_speed_mode = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::SAFE, - .target_spin_rev_per_s = 0.0}; + .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::SAFE}; FSM fsm; EXPECT_TRUE(fsm.is(boost::sml::state)); @@ -45,13 +43,11 @@ TEST(MoveFSMTest, test_transitions) control_params = MoveFSM::ControlParams{ .destination = Point(1, -3), .final_orientation = Angle::half(), - .final_speed = 0.0, .dribbler_mode = TbotsProto::DribblerMode::OFF, .ball_collision_type = TbotsProto::BallCollisionType::AVOID, .auto_chip_or_kick = AutoChipOrKick{AutoChipOrKickMode::OFF, 0}, .max_allowed_speed_mode = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::SAFE, - .target_spin_rev_per_s = 0.0}; + .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::SAFE}; fsm.process_event(MoveFSM::Update( control_params, TacticUpdate(robot, world, [](std::shared_ptr) {}))); EXPECT_TRUE(fsm.is(boost::sml::state)); diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic.cpp b/src/software/ai/hl/stp/tactic/move/move_tactic.cpp index b7ae2624e2..4f034e9067 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_tactic.cpp @@ -8,13 +8,11 @@ MoveTactic::MoveTactic() control_params{ .destination = Point(), .final_orientation = Angle::zero(), - .final_speed = 0.0, .dribbler_mode = TbotsProto::DribblerMode::OFF, .ball_collision_type = TbotsProto::BallCollisionType::AVOID, .auto_chip_or_kick = {AutoChipOrKickMode::OFF, 0}, .max_allowed_speed_mode = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, - .target_spin_rev_per_s = 0.0} + .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE} { for (RobotId id = 0; id < MAX_ROBOT_IDS; id++) { @@ -23,40 +21,34 @@ MoveTactic::MoveTactic() } void MoveTactic::updateControlParams( - Point destination, Angle final_orientation, double final_speed, - TbotsProto::DribblerMode dribbler_mode, + Point destination, Angle final_orientation, TbotsProto::DribblerMode dribbler_mode, TbotsProto::BallCollisionType ball_collision_type, AutoChipOrKick auto_chip_or_kick, TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, - TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode, - double target_spin_rev_per_s) + TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode) { // Update the control parameters stored by this Tactic control_params.destination = destination; control_params.final_orientation = final_orientation; - control_params.final_speed = final_speed; control_params.dribbler_mode = dribbler_mode; control_params.ball_collision_type = ball_collision_type; control_params.auto_chip_or_kick = auto_chip_or_kick; control_params.max_allowed_speed_mode = max_allowed_speed_mode; control_params.obstacle_avoidance_mode = obstacle_avoidance_mode; - control_params.target_spin_rev_per_s = target_spin_rev_per_s; } void MoveTactic::updateControlParams( - Point destination, Angle final_orientation, double final_speed, + Point destination, Angle final_orientation, TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode) { // Update the control parameters stored by this Tactic control_params.destination = destination; control_params.final_orientation = final_orientation; - control_params.final_speed = final_speed; control_params.dribbler_mode = TbotsProto::DribblerMode::OFF; control_params.ball_collision_type = TbotsProto::BallCollisionType::AVOID; control_params.auto_chip_or_kick = {AutoChipOrKickMode::OFF, 0}; control_params.max_allowed_speed_mode = max_allowed_speed_mode; control_params.obstacle_avoidance_mode = obstacle_avoidance_mode; - control_params.target_spin_rev_per_s = 0.0; } void MoveTactic::updatePrimitive(const TacticUpdate &tactic_update, bool reset_fsm) diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic.h b/src/software/ai/hl/stp/tactic/move/move_tactic.h index 7232304473..c795d12f74 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic.h +++ b/src/software/ai/hl/stp/tactic/move/move_tactic.h @@ -21,17 +21,15 @@ class MoveTactic : public Tactic * @param destination The destination to move to (in global coordinates) * @param final_orientation The final orientation the robot should have at * the destination - * @param final_speed The final speed the robot should have at the destination * @param dribbler_mode The dribbler mode * @param ball_collision_type how to navigate around the ball * @param auto_chip_or_kick The command to autochip or autokick * @param max_allowed_speed_mode The mode of maximum speed allowed * @param obstacle_avoidance_mode How safe we should be when avoiding obstacles, * particularly enemy robots - * @param target_spin_rev_per_s The target spin while moving in revolutions per second */ void updateControlParams( - Point destination, Angle final_orientation, double final_speed, + Point destination, Angle final_orientation, TbotsProto::DribblerMode dribbler_mode = TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType ball_collision_type = TbotsProto::BallCollisionType::AVOID, @@ -39,8 +37,7 @@ class MoveTactic : public Tactic TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode = - TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, - double target_spin_rev_per_s = 0.0); + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); /** * Updates the params assuming that the dribbler and chicker and while avoiding the @@ -49,13 +46,11 @@ class MoveTactic : public Tactic * @param destination The destination to move to (in global coordinates) * @param final_orientation The final orientation the robot should have at * the destination - * @param final_speed The final speed the robot should have at the destination * @param max_allowed_speed_mode The mode of maximum speed allowed * @param obstacle_avoidance_mode How safe we should be when avoiding obstacles, * particularly enemy robots */ void updateControlParams(Point destination, Angle final_orientation, - double final_speed, TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode, TbotsProto::ObstacleAvoidanceMode obstacle_avoidance_mode); diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp b/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp index d7917038ec..614db132ce 100644 --- a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp +++ b/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp @@ -33,7 +33,7 @@ TEST_F(MoveTacticTest, test_move_across_field) field.enemyDefenseArea().negXPosYCorner()}); auto tactic = std::make_shared(); - tactic->updateControlParams(destination, Angle::zero(), 0); + tactic->updateControlParams(destination, Angle::zero()); setTactic(1, tactic); std::vector terminating_validation_functions = { @@ -72,11 +72,11 @@ TEST_F(MoveTacticTest, test_autochip_move) field.enemyDefenseArea().negXPosYCorner()}); auto tactic = std::make_shared(); - tactic->updateControlParams( - destination, Angle::zero(), 0, TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::AUTOCHIP, 2.0}, - TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED, - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + tactic->updateControlParams(destination, Angle::zero(), TbotsProto::DribblerMode::OFF, + TbotsProto::BallCollisionType::ALLOW, + {AutoChipOrKickMode::AUTOCHIP, 2.0}, + TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED, + TbotsProto::ObstacleAvoidanceMode::SAFE); setTactic(1, tactic); std::vector terminating_validation_functions = { @@ -119,10 +119,10 @@ TEST_F(MoveTacticTest, test_autokick_move) auto tactic = std::make_shared(); tactic->updateControlParams( - destination, Angle::threeQuarter(), 0, TbotsProto::DribblerMode::OFF, + destination, Angle::threeQuarter(), TbotsProto::DribblerMode::OFF, TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::AUTOKICK, 3.0}, TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED, - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + TbotsProto::ObstacleAvoidanceMode::SAFE); setTactic(0, tactic); std::vector terminating_validation_functions = { @@ -161,11 +161,11 @@ TEST_F(MoveTacticTest, test_spinning_move_clockwise) auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); auto tactic = std::make_shared(); - tactic->updateControlParams( - destination, Angle::zero(), 0, TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0.0}, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::SAFE, 0.0); + tactic->updateControlParams(destination, Angle::zero(), TbotsProto::DribblerMode::OFF, + TbotsProto::BallCollisionType::ALLOW, + {AutoChipOrKickMode::OFF, 0.0}, + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::SAFE); setTactic(0, tactic); std::vector terminating_validation_functions = { @@ -208,11 +208,11 @@ TEST_F(MoveTacticTest, test_spinning_move_counter_clockwise) auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); auto tactic = std::make_shared(); - tactic->updateControlParams( - destination, Angle::half(), 0, TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::OFF, 0.0}, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::SAFE, -4.0); + tactic->updateControlParams(destination, Angle::half(), TbotsProto::DribblerMode::OFF, + TbotsProto::BallCollisionType::ALLOW, + {AutoChipOrKickMode::OFF, 0.0}, + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::SAFE); setTactic(0, tactic); std::vector terminating_validation_functions = { diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp index bc52f395a0..407bde1ff0 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp @@ -102,13 +102,11 @@ void ShadowEnemyFSM::blockShot(const Update &event, MoveFSM::ControlParams control_params{ .destination = position_to_block, .final_orientation = face_ball_orientation, - .final_speed = 0.0, .dribbler_mode = TbotsProto::DribblerMode::OFF, .ball_collision_type = TbotsProto::BallCollisionType::AVOID, .auto_chip_or_kick = AutoChipOrKick{AutoChipOrKickMode::OFF, 0}, .max_allowed_speed_mode = TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, - .target_spin_rev_per_s = 0.0}; + .obstacle_avoidance_mode = TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE}; processEvent(MoveFSM::Update(control_params, event.common)); } diff --git a/src/software/ai/hl/stp/tactic/stop/stop_fsm.cpp b/src/software/ai/hl/stp/tactic/stop/stop_fsm.cpp deleted file mode 100644 index b8728aaa13..0000000000 --- a/src/software/ai/hl/stp/tactic/stop/stop_fsm.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "software/ai/hl/stp/tactic/stop/stop_fsm.h" - -void StopFSM::updateStop(const Update& event) -{ - event.common.set_primitive(std::make_unique()); -} - -bool StopFSM::stopDone(const Update& event) -{ - return robotStopped(event.common.robot); -} diff --git a/src/software/ai/hl/stp/tactic/stop/stop_tactic.cpp b/src/software/ai/hl/stp/tactic/stop/stop_tactic.cpp deleted file mode 100644 index d6255bce30..0000000000 --- a/src/software/ai/hl/stp/tactic/stop/stop_tactic.cpp +++ /dev/null @@ -1,26 +0,0 @@ -#include "software/ai/hl/stp/tactic/stop/stop_tactic.h" - -#include - -StopTactic::StopTactic() : Tactic(std::set()), fsm_map() -{ - for (RobotId id = 0; id < MAX_ROBOT_IDS; id++) - { - fsm_map[id] = std::make_unique>(StopFSM()); - } -} - -void StopTactic::accept(TacticVisitor &visitor) const -{ - visitor.visit(*this); -} - -void StopTactic::updatePrimitive(const TacticUpdate &tactic_update, bool reset_fsm) -{ - if (reset_fsm) - { - fsm_map[tactic_update.robot.id()] = std::make_unique>(StopFSM()); - } - fsm_map.at(tactic_update.robot.id()) - ->process_event(StopFSM::Update({}, tactic_update)); -} diff --git a/src/software/ai/hl/stp/tactic/tactic_factory.cpp b/src/software/ai/hl/stp/tactic/tactic_factory.cpp index e7e9957c92..73f51d2a66 100644 --- a/src/software/ai/hl/stp/tactic/tactic_factory.cpp +++ b/src/software/ai/hl/stp/tactic/tactic_factory.cpp @@ -29,7 +29,7 @@ std::shared_ptr createTactic(const TbotsProto::Tactic &tactic_proto, PROTO_CREATE_TACTIC_CASE(PivotKick, pivot_kick) PROTO_CREATE_TACTIC_CASE(Receiver, receiver) PROTO_CREATE_TACTIC_CASE(ShadowEnemy, shadow_enemy) - PROTO_CREATE_TACTIC_CASE(Stop, stop) + PROTO_CREATE_TACTIC_CASE(Halt, halt) case TbotsProto::Tactic::TACTIC_NOT_SET: { LOG(FATAL) << "Tactic not set"; @@ -141,11 +141,10 @@ std::shared_ptr createTactic(const TbotsProto::MoveTactic &tactic_proto, auto tactic = std::make_shared(); tactic->updateControlParams( createPoint(tactic_proto.destination()), - createAngle(tactic_proto.final_orientation()), tactic_proto.final_speed(), - tactic_proto.dribbler_mode(), tactic_proto.ball_collision_type(), + createAngle(tactic_proto.final_orientation()), tactic_proto.dribbler_mode(), + tactic_proto.ball_collision_type(), createAutoChipOrKick(tactic_proto.auto_chip_or_kick()), - tactic_proto.max_allowed_speed_mode(), tactic_proto.obstacle_avoidance_mode(), - tactic_proto.target_spin_rev_per_s()); + tactic_proto.max_allowed_speed_mode(), tactic_proto.obstacle_avoidance_mode()); return tactic; } @@ -203,10 +202,10 @@ std::shared_ptr createTactic(const TbotsProto::ShadowEnemyTactic &tactic return tactic; } -std::shared_ptr createTactic(const TbotsProto::StopTactic &tactic_proto, +std::shared_ptr createTactic(const TbotsProto::HaltTactic &tactic_proto, TbotsProto::AiConfig ai_config) { - auto tactic = std::make_shared(); + auto tactic = std::make_shared(); return tactic; } diff --git a/src/software/ai/hl/stp/tactic/tactic_factory.h b/src/software/ai/hl/stp/tactic/tactic_factory.h index 5da161d4af..a3967b69bc 100644 --- a/src/software/ai/hl/stp/tactic/tactic_factory.h +++ b/src/software/ai/hl/stp/tactic/tactic_factory.h @@ -42,7 +42,7 @@ std::shared_ptr createTactic(const TbotsProto::ReceiverTactic &tactic_pr TbotsProto::AiConfig ai_config); std::shared_ptr createTactic(const TbotsProto::ShadowEnemyTactic &tactic_proto, TbotsProto::AiConfig ai_config); -std::shared_ptr createTactic(const TbotsProto::StopTactic &tactic_proto, +std::shared_ptr createTactic(const TbotsProto::HaltTactic &tactic_proto, TbotsProto::AiConfig ai_config); /** diff --git a/src/software/ai/hl/stp/tactic/tactic_visitor.h b/src/software/ai/hl/stp/tactic/tactic_visitor.h index 5ef7fc69a1..ea5c6e5ab0 100644 --- a/src/software/ai/hl/stp/tactic/tactic_visitor.h +++ b/src/software/ai/hl/stp/tactic/tactic_visitor.h @@ -23,7 +23,7 @@ class PenaltySetupTactic; class PivotKickTactic; class ReceiverTactic; class ShadowEnemyTactic; -class StopTactic; +class HaltTactic; class StopTestTactic; class MoveGoalieToGoalLineTactic; class PrepareKickoffMoveTactic; @@ -65,7 +65,7 @@ class TacticVisitor virtual void visit(const PivotKickTactic &tactic) = 0; virtual void visit(const ReceiverTactic &tactic) = 0; virtual void visit(const ShadowEnemyTactic &tactic) = 0; - virtual void visit(const StopTactic &tactic) = 0; + virtual void visit(const HaltTactic &tactic) = 0; virtual void visit(const StopTestTactic &tactic) = 0; virtual void visit(const MoveGoalieToGoalLineTactic &tactic) = 0; virtual void visit(const PrepareKickoffMoveTactic &tactic) = 0; diff --git a/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp b/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp index daaafdfb7d..7b58c68d57 100644 --- a/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp +++ b/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp @@ -68,7 +68,7 @@ namespace std::make_tuple(std::make_shared(ai_config), std::set(), std::set()), - std::make_tuple(std::make_shared(), + std::make_tuple(std::make_shared(), std::set(), std::set()), std::make_tuple( diff --git a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp index 2e72672309..cbedffc25c 100644 --- a/src/software/ai/motion_constraint/motion_constraint_visitor.cpp +++ b/src/software/ai/motion_constraint/motion_constraint_visitor.cpp @@ -46,7 +46,7 @@ void MotionConstraintVisitor::visit(const PrepareKickoffMoveTactic &tactic) TbotsProto::MotionConstraint::ENEMY_HALF_WITHOUT_CENTRE_CIRCLE); } -void MotionConstraintVisitor::visit(const StopTactic &tactic) {} +void MotionConstraintVisitor::visit(const HaltTactic &tactic) {} void MotionConstraintVisitor::visit(const PenaltyKickTactic &tactic) { diff --git a/src/software/ai/motion_constraint/motion_constraint_visitor.h b/src/software/ai/motion_constraint/motion_constraint_visitor.h index 5aba45c2b5..8c913c1ae6 100644 --- a/src/software/ai/motion_constraint/motion_constraint_visitor.h +++ b/src/software/ai/motion_constraint/motion_constraint_visitor.h @@ -26,7 +26,7 @@ class MotionConstraintVisitor : public TacticVisitor void visit(const ChipTactic &tactic) override; void visit(const KickTactic &tactic) override; void visit(const KickoffChipTactic &tactic) override; - void visit(const StopTactic &tactic) override; + void visit(const HaltTactic &tactic) override; void visit(const PenaltyKickTactic &tactic) override; void visit(const PenaltySetupTactic &tactic) override; void visit(const ReceiverTactic &tactic) override; diff --git a/src/software/ai/navigator/trajectory/simulated_hrvo_test.py b/src/software/ai/navigator/trajectory/simulated_hrvo_test.py index 02af4fa0da..cc5dc5d911 100644 --- a/src/software/ai/navigator/trajectory/simulated_hrvo_test.py +++ b/src/software/ai/navigator/trajectory/simulated_hrvo_test.py @@ -206,7 +206,6 @@ def hrvo_setup( if friendly_robots_final_orientations else desired_orientation ), - 0, params=blue_params, ) @@ -217,7 +216,7 @@ def hrvo_setup( for index, destination in enumerate(enemy_robots_destinations): yellow_params = get_move_update_control_params( - index, destination, tbots.Angle.fromRadians(0), 0, params=yellow_params + index, destination, tbots.Angle.fromRadians(0), params=yellow_params ) simulated_test_runner.set_tactics(yellow_params, False) @@ -541,7 +540,6 @@ def get_move_update_control_params( robot_id: int, destination: tbots.Point, desired_orientation: tbots.Angle, - final_speed: float, params: AssignedTacticPlayControlParams = None, ): """Constructs the control params for a Move Tactic for a single robot @@ -551,7 +549,6 @@ def get_move_update_control_params( :param robot_id: the id of the robot who will be assigned these params :param destination: the destination of the robot :param desired_orientation: the desired orientation of the robot - :param final_speed: the final speed of the robot :param params: AssignedTacticPlayControlParams message if not None, add this robot's params to this else, create a new message and add @@ -562,12 +559,10 @@ def get_move_update_control_params( MoveTactic( destination=Point(x_meters=destination.x(), y_meters=destination.y()), final_orientation=Angle(radians=desired_orientation.toRadians()), - final_speed=final_speed, dribbler_mode=DribblerMode.OFF, ball_collision_type=BallCollisionType.ALLOW, auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, - target_spin_rev_per_s=0.0, obstacle_avoidance_mode=ObstacleAvoidanceMode.AGGRESSIVE, ) ) diff --git a/src/software/autoref/run_autoref.sh b/src/software/autoref/run_autoref.sh index 75f21be914..a4f3c8a61b 100755 --- a/src/software/autoref/run_autoref.sh +++ b/src/software/autoref/run_autoref.sh @@ -3,7 +3,7 @@ # Runs the Tigers Autoref binary that is set up by "setup_software.sh". By default, wrapper runs the autoreferee in headless, active mode. If an additional argument is passed in to the script, it is forwarded to the binary. cd "/opt/tbotspython/autoReferee/" -export JAVA_HOME="/usr/lib/jvm/jdk-17" +export JAVA_HOME="/opt/tbotspython/bin/jdk" bin/autoReferee -a $@ diff --git a/src/software/jetson_nano/BUILD b/src/software/embedded/BUILD similarity index 58% rename from src/software/jetson_nano/BUILD rename to src/software/embedded/BUILD index 2714cd3435..d43be47e00 100644 --- a/src/software/jetson_nano/BUILD +++ b/src/software/embedded/BUILD @@ -1,5 +1,36 @@ package(default_visibility = ["//visibility:public"]) +load("@bazel_skylib//rules:common_settings.bzl", "string_flag") + +config_setting( + name = "build_nano", + flag_values = { + ":host_platform": "NANO", + }, +) + +config_setting( + name = "build_pi", + flag_values = { + ":host_platform": "PI", + }, +) + +string_flag( + name = "host_platform", + build_setting_default = "PI", + values = [ + "PI", + "NANO", + ], +) + +cc_library( + name = "platform", + hdrs = ["platform.h"], + deps = ["//software/util/make_enum"], +) + cc_library( name = "primitive_executor", srcs = ["primitive_executor.cpp"], @@ -7,7 +38,6 @@ cc_library( deps = [ "//proto:tbots_cc_proto", "//proto/primitive:primitive_msg_factory", - "//shared:constants", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/ai/navigator/trajectory:trajectory_path", "//software/math:math_functions", @@ -23,10 +53,10 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", - "//software/jetson_nano/redis", - "//software/jetson_nano/services:motor", - "//software/jetson_nano/services:power", - "//software/jetson_nano/services/network", + "//software/embedded/redis", + "//software/embedded/services:motor", + "//software/embedded/services:power", + "//software/embedded/services/network", "//software/logger:network_logger", "//software/tracy:tracy_constants", "//software/util/scoped_timespec_timer", @@ -37,8 +67,15 @@ cc_library( cc_library( name = "gpio", - srcs = ["gpio.cpp"], - hdrs = ["gpio.h"], + srcs = [ + "gpio_char_dev.cpp", + "gpio_sysfs.cpp", + ], + hdrs = [ + "gpio.h", + "gpio_char_dev.h", + "gpio_sysfs.h", + ], deps = [ "//software/logger", "//software/logger:network_logger", @@ -53,16 +90,19 @@ cc_binary( "-lpthread", "-lrt", ], + target_compatible_with = [ + "@platforms//cpu:aarch64", + "@platforms//os:linux", + ], deps = [ ":thunderloop", - "//shared:constants", "@boost//:program_options", ], ) sh_binary( - name = "setup_nano", - srcs = ["setup_nano.sh"], + name = "setup_robot_software_deps", + srcs = ["setup_robot_software_deps.sh"], ) cc_test( diff --git a/src/software/jetson_nano/ansible/BUILD b/src/software/embedded/ansible/BUILD similarity index 62% rename from src/software/jetson_nano/ansible/BUILD rename to src/software/embedded/ansible/BUILD index c8ce2df770..625c8eb0d1 100644 --- a/src/software/jetson_nano/ansible/BUILD +++ b/src/software/embedded/ansible/BUILD @@ -19,13 +19,14 @@ py_binary( data = [ ":playbooks", ":scripts", - "//software/jetson_nano:setup_nano", - "//software/jetson_nano:thunderloop_main", - "//software/jetson_nano/linux_configs:device_tree", - "//software/jetson_nano/linux_configs:extlinux", - "//software/jetson_nano/linux_configs/systemd:systemd_files", - "//software/jetson_nano/redis", - "//software/jetson_nano/services:robot_auto_test", + ":tasks", + "//software/embedded:setup_robot_software_deps", + "//software/embedded:thunderloop_main", + "//software/embedded/linux_configs/jetson_nano:jetson_nano_files", + "//software/embedded/linux_configs/pi:pi_files", + "//software/embedded/linux_configs/systemd:systemd_files", + "//software/embedded/redis", + "//software/embedded/services:robot_auto_test", "//software/power:powerloop_tar", ], deps = [ @@ -46,3 +47,10 @@ filegroup( "scripts/**/*.py", ]), ) + +filegroup( + name = "tasks", + data = glob([ + "tasks/**/*.yml", + ]), +) diff --git a/src/software/jetson_nano/ansible/playbooks/deploy_powerboard.yml b/src/software/embedded/ansible/playbooks/deploy_powerboard.yml similarity index 100% rename from src/software/jetson_nano/ansible/playbooks/deploy_powerboard.yml rename to src/software/embedded/ansible/playbooks/deploy_powerboard.yml diff --git a/src/software/jetson_nano/ansible/playbooks/deploy_nano.yml b/src/software/embedded/ansible/playbooks/deploy_robot_software.yml similarity index 73% rename from src/software/jetson_nano/ansible/playbooks/deploy_nano.yml rename to src/software/embedded/ansible/playbooks/deploy_robot_software.yml index 4e33513faa..a41c667f2c 100644 --- a/src/software/jetson_nano/ansible/playbooks/deploy_nano.yml +++ b/src/software/embedded/ansible/playbooks/deploy_robot_software.yml @@ -65,13 +65,6 @@ when: "'sync' in actions" tags: always - - name: Unzipping python binaries - command: 'unzip ~/thunderbots_binaries/*.zip' - register: result - args: - chdir: ~/thunderbots_binaries - when: "'sync' in actions" - - name: Start Services become: true become_method: sudo @@ -85,22 +78,3 @@ loop_var: service_name when: "'start' in actions" tags: always - - - name: Enable WiFi waiter service - become: true - become_method: sudo - service: name=NetworkManager-wait-online enabled=yes - - - name: Disable Announcements - become: true - become_method: sudo - ansible.builtin.systemd: - name: "{{ service_name }}" - enabled: no - masked: no - state: stopped - with_items: ["wifi_announcement", "ethernet_announcement"] - loop_control: - loop_var: service_name - tags: always - failed_when: false diff --git a/src/software/jetson_nano/ansible/playbooks/example_playbook.yml b/src/software/embedded/ansible/playbooks/example_playbook.yml similarity index 100% rename from src/software/jetson_nano/ansible/playbooks/example_playbook.yml rename to src/software/embedded/ansible/playbooks/example_playbook.yml diff --git a/src/software/jetson_nano/ansible/playbooks/misc.yml b/src/software/embedded/ansible/playbooks/misc.yml similarity index 100% rename from src/software/jetson_nano/ansible/playbooks/misc.yml rename to src/software/embedded/ansible/playbooks/misc.yml diff --git a/src/software/jetson_nano/ansible/playbooks/robot_auto_test_playbook.yml b/src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml similarity index 100% rename from src/software/jetson_nano/ansible/playbooks/robot_auto_test_playbook.yml rename to src/software/embedded/ansible/playbooks/robot_auto_test_playbook.yml diff --git a/src/software/embedded/ansible/playbooks/setup_nano.yml b/src/software/embedded/ansible/playbooks/setup_nano.yml new file mode 100644 index 0000000000..fb8c0365cf --- /dev/null +++ b/src/software/embedded/ansible/playbooks/setup_nano.yml @@ -0,0 +1,137 @@ +--- + +- name: Setting up the jetson Nano + hosts: THUNDERBOTS_HOSTS + + tasks: + + - name: Check internet connection + tags: dependencies + import_tasks: ../tasks/check_internet.yml + + - name: Enable passwordless sudo for rsync + tags: + - dependencies + - configure_nano + import_tasks: ../tasks/enable_password_less_sudo_for_rsync.yml + + - name: Setup Robot Software dependencies + tags: + - dependencies + import_tasks: ../tasks/setup_robot_software_deps.yml + + - name: Setup systemd + tags: + - dependencies + - configure_nano + import_tasks: ../tasks/setup_systemd.yml + + - name: Add user robot to dialout + tags: + - configure_nano + - dependencies + import_tasks: ../tasks/add_user_to_dialout.yml + + - name: Run Jetson Clocks + become: true + become_method: sudo + shell: /usr/bin/jetson_clocks + tags: configure_nano + + # UART setup + - name: Stop nvgetty + become: true + become_method: sudo + command: 'systemctl stop nvgetty' + tags: configure_nano + + - name: Disable nvgetty + become: true + become_method: sudo + command: 'systemctl disable nvgetty' + tags: configure_nano + + - name: Setup udevadm trigger + become: true + become_method: sudo + command: 'udevadm trigger' + tags: configure_nano + + - name: Add user robot to dialout + ansible.builtin.import_tasks: ../tasks/add_user_to_dialout.yml + tags: configure_nano + + # Device tree setup + - name: Sync Device Tree + become: true + become_method: sudo + tags: + - sync + - device_tree + ansible.posix.synchronize: + src: ../../../linux_configs/jetson_nano/device_tree.zip + dest: ~/ + recursive: yes + copy_links: yes + + - name: Sync extlinux conf + become: true + become_method: sudo + tags: + - sync + - device_tree + ansible.posix.synchronize: + src: ../../../linux_configs/jetson_nano/extlinux.conf + dest: ~/ + recursive: yes + copy_links: yes + + - name: Unzip device tree + tags: + - sync + - device_tree + command: 'unzip -o ~/device_tree.zip' + register: result + args: + chdir: ~/ + + - name: Compile device tree + tags: + - sync + - device_tree + shell: 'dtc -q -I dts -O dtb ~/device_tree.dts > device_tree.dtb' + register: result + args: + chdir: ~/ + + - name: Move compiled device tree binary to boot path + become_method: sudo + become: true + tags: + - sync + - device_tree + shell: 'mv /home/robot/device_tree.dtb /boot/kernel_tegra210-p3448-0000-p3449-0000-b00-user-custom.dtb' + register: result + args: + chdir: ~/ + + - name: Move extlinux file + become_method: sudo + become: true + tags: + - sync + - device_tree + shell: 'mv /home/robot/extlinux.conf /boot/extlinux/extlinux.conf' + register: result + args: + chdir: ~/ + + - name: Reboot + tags: always + ansible.builtin.import_tasks: + file: ../tasks/reboot.yml + + - name: Setup success + tags: always + debug: + msg: "Setup complete for {{ inventory_hostname }}!" diff --git a/src/software/embedded/ansible/playbooks/setup_pi.yml b/src/software/embedded/ansible/playbooks/setup_pi.yml new file mode 100644 index 0000000000..857e076117 --- /dev/null +++ b/src/software/embedded/ansible/playbooks/setup_pi.yml @@ -0,0 +1,128 @@ +--- + +- name: Setting up the Raspberry Pi + hosts: THUNDERBOTS_HOSTS + + tasks: + + - name: Check internet connection + tags: dependencies + ansible.builtin.import_tasks: + file: ../tasks/check_internet.yml + + - name: Enable passwordless sudo for rsync + tags: + - dependencies + - configure_pi + ansible.builtin.import_tasks: + file: ../tasks/enable_password_less_sudo_for_rsync.yml + + - name: Setup Robot Software dependencies + tags: + - dependencies + ansible.builtin.import_tasks: + file: ../tasks/setup_robot_software_deps.yml + + - name: Setup systemd + tags: + - dependencies + - configure_pi + ansible.builtin.import_tasks: + file: ../tasks/setup_systemd.yml + + - name: Add user robot to dialout + tags: + - configure_pi + - dependencies + ansible.builtin.import_tasks: + file: ../tasks/add_user_to_dialout.yml + + - name: Disable pcie_aspm + tags: + - configure_pi + block: + - name: Backup cmdline.txt + become: true + become_method: sudo + ansible.builtin.copy: + src: /boot/firmware/cmdline.txt + dest: /boot/firmware/cmdline.txt.bak + remote_src: true + + - name: Check if pcie_aspm is disabled + ansible.builtin.shell: 'grep -q "pcie_aspm=off" /boot/firmware/cmdline.txt' + ignore_errors: true + register: pcie_aspm_disabled + + - name: Disable pcie_aspm + when: pcie_aspm_disabled.rc != 0 + become: true + become_method: sudo + ansible.builtin.lineinfile: + path: /boot/firmware/cmdline.txt + regexp: ^(\s*console.*)$ + backrefs: yes + line: '\1 pcie_aspm=off' + state: present + + - name: Set up SPI and Wifi drivers + tags: configure_pi + block: + - name: Sync setup files + ansible.posix.synchronize: + src: ../../linux_configs/pi/setup.zip + dest: ~/ + copy_links: yes + + - name: Unzip setup files + ansible.builtin.unarchive: + src: ~/setup.zip + dest: ~/ + remote_src: true + + - name: Set up Wifi drivers + become: true + become_method: sudo + shell: 'mv /home/{{ ansible_user }}/iwlwifi* /lib/firmware/' + + - name: Set up systemd Wifi interface name rule + ansible.builtin.import_tasks: + file: ../tasks/setup_wifi_interface.yml + + - name: Set up SPI overlay + become: true + become_method: sudo + shell: 'mv /home/{{ ansible_user }}/spi0-5cs.dtbo /boot/firmware/overlays/' + + - name: Update config.txt to select custom SPI overlay + become: true + become_method: sudo + ansible.posix.synchronize: + src: ../../linux_configs/pi/config.txt + dest: /boot/firmware/ + copy_links: yes + rsync_opts: + - "--no-perms" + - "--no-o" + - "--no-g" + + - name: Change hostname + tags: update_hostname + ansible.builtin.import_tasks: + file: ../tasks/set_user_hostname.yml + + - name: Reboot + tags: + - dependencies + - configure_pi + - update_hostname + ansible.builtin.import_tasks: + file: ../tasks/reboot.yml + + - name: Success + tags: + - dependencies + - configure_pi + - update_hostname + ansible.builtin.debug: + msg: "Raspberry Pi setup complete for {{ inventory_hostname }}" diff --git a/src/software/embedded/ansible/playbooks/shared_setup.yml b/src/software/embedded/ansible/playbooks/shared_setup.yml new file mode 100644 index 0000000000..2ec71245f6 --- /dev/null +++ b/src/software/embedded/ansible/playbooks/shared_setup.yml @@ -0,0 +1,6 @@ +--- + +- name: Success msg + debug: + msg: + - "[Robot ID = {{ inventory_hostname }}]" diff --git a/src/software/embedded/ansible/requirements.in b/src/software/embedded/ansible/requirements.in new file mode 100644 index 0000000000..22c3a7550c --- /dev/null +++ b/src/software/embedded/ansible/requirements.in @@ -0,0 +1 @@ +ansible==10.5.0 diff --git a/src/software/jetson_nano/ansible/requirements_lock.txt b/src/software/embedded/ansible/requirements_lock.txt similarity index 96% rename from src/software/jetson_nano/ansible/requirements_lock.txt rename to src/software/embedded/ansible/requirements_lock.txt index 8c1ce01c19..b7223bb4b9 100644 --- a/src/software/jetson_nano/ansible/requirements_lock.txt +++ b/src/software/embedded/ansible/requirements_lock.txt @@ -1,14 +1,16 @@ # -# This file is autogenerated by pip-compile with Python 3.10 +# This file is autogenerated by pip-compile with Python 3.12 # by the following command: # -# bazel run //software/jetson_nano/ansible:requirements.update +# bazel run //software/embedded/ansible:requirements.update # -ansible==5.3.0 \ - --hash=sha256:50020dab43f6c59debdeb57f45c90ec6db78d4fa574edc6d75bc52e05cbd3639 - # via -r software/jetson_nano/ansible/requirements.in -ansible-core==2.12.10 \ - --hash=sha256:feb1df61738cfc1f5e893b42a2ec1a7de32977d67e86707b45eb63d0c5c3c236 +ansible==10.5.0 \ + --hash=sha256:1d10bddba58f1edd0fe0b8e0387e0fafc519535066bb3c919c33b6ea3ec32a0f \ + --hash=sha256:ba2045031a7d60c203b6e5fe1f8eaddd53ae076f7ada910e636494384135face + # via -r software/embedded/ansible/requirements.in +ansible-core==2.17.5 \ + --hash=sha256:10f165b475cf2bc8d886e532cadb32c52ee6a533649793101d3166bca9bd3ea3 \ + --hash=sha256:ae7f51fd13dc9d57c9bcd43ef23f9c255ca8f18f4b5c0011a4f9b724d92c5a8e # via ansible cffi==1.16.0 \ --hash=sha256:0c9ef6ff37e974b73c25eecc13952c55bceed9112be2d9d938ded8e856138bcc \ diff --git a/src/software/jetson_nano/ansible/run_ansible.py b/src/software/embedded/ansible/run_ansible.py similarity index 98% rename from src/software/jetson_nano/ansible/run_ansible.py rename to src/software/embedded/ansible/run_ansible.py index 4ddbe0a5b8..bd62b9fb25 100644 --- a/src/software/jetson_nano/ansible/run_ansible.py +++ b/src/software/embedded/ansible/run_ansible.py @@ -5,6 +5,8 @@ from ansible.parsing.dataloader import DataLoader from ansible.inventory.manager import InventoryManager from ansible.vars.manager import VariableManager +from ansible.plugins.loader import init_plugin_loader + import os import argparse import subprocess @@ -20,6 +22,7 @@ # loads variables, inventory, and play into Ansible API, then runs it def ansible_runner(playbook: str, options: dict = {}): + init_plugin_loader() loader = DataLoader() print("starting ansible run") diff --git a/src/software/embedded/ansible/tasks/add_user_to_dialout.yml b/src/software/embedded/ansible/tasks/add_user_to_dialout.yml new file mode 100644 index 0000000000..bfb93ed250 --- /dev/null +++ b/src/software/embedded/ansible/tasks/add_user_to_dialout.yml @@ -0,0 +1,8 @@ +--- + +- name: Add user robot to dialout + become: true + become_method: sudo + tags: + - add_user_robot_to_dialout + command: 'adduser robot dialout' diff --git a/src/software/embedded/ansible/tasks/check_internet.yml b/src/software/embedded/ansible/tasks/check_internet.yml new file mode 100644 index 0000000000..b45d16d3a9 --- /dev/null +++ b/src/software/embedded/ansible/tasks/check_internet.yml @@ -0,0 +1,8 @@ +--- + +- name: check internet connection + tags: check_internet + uri: + url: "https://google.com" + timeout: 30 + status_code: 200 diff --git a/src/software/embedded/ansible/tasks/enable_password_less_sudo_for_rsync.yml b/src/software/embedded/ansible/tasks/enable_password_less_sudo_for_rsync.yml new file mode 100644 index 0000000000..e9f37c86af --- /dev/null +++ b/src/software/embedded/ansible/tasks/enable_password_less_sudo_for_rsync.yml @@ -0,0 +1,10 @@ +--- + +- name: enable passwordless sudo for rsync + become: true + become_method: sudo + lineinfile: + path: /etc/sudoers + state: present + insertafter: EOF + line: '{{ ansible_user }} ALL=NOPASSWD:/usr/bin/rsync' diff --git a/src/software/embedded/ansible/tasks/reboot.yml b/src/software/embedded/ansible/tasks/reboot.yml new file mode 100644 index 0000000000..56b3500a60 --- /dev/null +++ b/src/software/embedded/ansible/tasks/reboot.yml @@ -0,0 +1,14 @@ +--- + +- name: Reboot + become: true + become_method: sudo + register: res + reboot: + msg: "Reboot initiated by Ansible" + connect_timeout: 20 + reboot_timeout: 1200 + pre_reboot_delay: 0 + post_reboot_delay: 10 + test_command: whoami + diff --git a/src/software/embedded/ansible/tasks/set_user_hostname.yml b/src/software/embedded/ansible/tasks/set_user_hostname.yml new file mode 100644 index 0000000000..1eb65fd60c --- /dev/null +++ b/src/software/embedded/ansible/tasks/set_user_hostname.yml @@ -0,0 +1,18 @@ +--- + +- name: Set user hostname + become: true + become_method: sudo + tags: + - set_hostname + when: new_hostname is defined + block: + - name: Set hostname + ansible.builtin.hostname: + name: "{{ new_hostname }}" + - name: Update /etc/hosts + ansible.builtin.lineinfile: + path: /etc/hosts + regexp: ^(127\.0\.1\.1\s*) + backrefs: yes + line: '\1{{ new_hostname }}' diff --git a/src/software/embedded/ansible/tasks/setup_robot_software_deps.yml b/src/software/embedded/ansible/tasks/setup_robot_software_deps.yml new file mode 100644 index 0000000000..e9097fe218 --- /dev/null +++ b/src/software/embedded/ansible/tasks/setup_robot_software_deps.yml @@ -0,0 +1,30 @@ +--- + +- name: Sync Setup Script + become: true + become_method: sudo + ansible.posix.synchronize: + src: ../../setup_robot_software_deps.sh + dest: ~/ + recursive: yes + copy_links: yes + +# Output is streamed to target host's (Jetson/Pi's /tmp/setup.log) +- name: Running the setup script, this will take a while + become_method: sudo + become: true + command: '/home/{{ ansible_user }}/setup_robot_software_deps.sh >& /tmp/setup.log' + register: result + args: + chdir: ~/ + +- name: Configure Redis persistence setting + become: true + become_method: sudo + ansible.builtin.lineinfile: + path: /etc/redis/redis.conf + state: present + regexp: ^(appendonly) + line: 'appendonly yes' + backrefs: yes + diff --git a/src/software/embedded/ansible/tasks/setup_systemd.yml b/src/software/embedded/ansible/tasks/setup_systemd.yml new file mode 100644 index 0000000000..c40695119a --- /dev/null +++ b/src/software/embedded/ansible/tasks/setup_systemd.yml @@ -0,0 +1,41 @@ +--- + +- name: Run systemd setup + block: + - name: Delete everything on ~/ + file: + state: absent + path: /home/robot/thunderbots_binaries + become_method: sudo + become: true + register: result + + - name: Sync Thunderloop + ansible.posix.synchronize: + src: ../../thunderloop_main + dest: ~/thunderbots_binaries/ + copy_links: yes + + - name: Sync Thunderloop systemd file + become_method: sudo + become: true + register: res + ansible.posix.synchronize: + src: ../../linux_configs/systemd/thunderloop.service + dest: /etc/systemd/system + copy_links: yes + + # NOTE: "Enabling systems" means they will start on boot + - name: Enable system services + become: true + become_method: sudo + ansible.builtin.systemd: + name: thunderloop + enabled: yes + masked: no + daemon_reload: yes + + - name: Enable WiFi waiter service + become: true + become_method: sudo + service: name=NetworkManager-wait-online enabled=yes diff --git a/src/software/embedded/ansible/tasks/setup_wifi_interface.yml b/src/software/embedded/ansible/tasks/setup_wifi_interface.yml new file mode 100644 index 0000000000..de5e1926b4 --- /dev/null +++ b/src/software/embedded/ansible/tasks/setup_wifi_interface.yml @@ -0,0 +1,20 @@ +--- +- name: Set up systemd WiFi 5 interface name rule + become: true + become_method: sudo + ansible.posix.synchronize: + src: ../../linux_configs/systemd/10-persistent-wifi5-net.link + dest: /etc/systemd/network/ + copy_links: yes + tags: + - setup_wifi_interface + +- name: Set up systemd WiFi 6 interface name rule + become: true + become_method: sudo + ansible.posix.synchronize: + src: ../../linux_configs/systemd/10-persistent-wifi6-net.link + dest: /etc/systemd/network/ + copy_links: yes + tags: + - setup_wifi_interface diff --git a/src/software/jetson_nano/battery_test.cpp b/src/software/embedded/battery_test.cpp similarity index 94% rename from src/software/jetson_nano/battery_test.cpp rename to src/software/embedded/battery_test.cpp index 4b29e23f40..a0604312c4 100644 --- a/src/software/jetson_nano/battery_test.cpp +++ b/src/software/embedded/battery_test.cpp @@ -3,7 +3,7 @@ #include #include -#include "thunderloop.h" +#include "software/embedded/thunderloop.h" TEST(TestBattery, is_power_stable) { diff --git a/src/software/embedded/constants/BUILD b/src/software/embedded/constants/BUILD new file mode 100644 index 0000000000..4953af9497 --- /dev/null +++ b/src/software/embedded/constants/BUILD @@ -0,0 +1,17 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "constants", + hdrs = [ + "constants.h", + "jetson_constants.h", + "pi_constants.h", + ], + defines = select({ + "//software/embedded:build_nano": ["NANO"], + "//software/embedded:build_pi": ["PI"], + }), + deps = [ + "//software/embedded:platform", + ], +) diff --git a/src/software/embedded/constants/constants.h b/src/software/embedded/constants/constants.h new file mode 100644 index 0000000000..3cf306e034 --- /dev/null +++ b/src/software/embedded/constants/constants.h @@ -0,0 +1,7 @@ +#pragma once + +#ifdef NANO +#include "software/embedded/constants/jetson_constants.h" +#elif PI +#include "software/embedded/constants/pi_constants.h" +#endif diff --git a/src/software/embedded/constants/jetson_constants.h b/src/software/embedded/constants/jetson_constants.h new file mode 100644 index 0000000000..64b608cdc5 --- /dev/null +++ b/src/software/embedded/constants/jetson_constants.h @@ -0,0 +1,13 @@ +#pragma once + +#include "software/embedded/platform.h" + +constexpr const char SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO[] = "51"; +constexpr const char SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO[] = "76"; +constexpr const char MOTOR_DRIVER_RESET_GPIO[] = "168"; +constexpr const char DRIVER_CONTROL_ENABLE_GPIO[] = "194"; + +// Path to the CPU thermal zone temperature file +constexpr const char CPU_TEMP_FILE_PATH[] = "/sys/class/thermal/thermal_zone1/temp"; + +constexpr Platform PLATFORM = Platform::JETSON_NANO; diff --git a/src/software/embedded/constants/pi_constants.h b/src/software/embedded/constants/pi_constants.h new file mode 100644 index 0000000000..f0cfe2914d --- /dev/null +++ b/src/software/embedded/constants/pi_constants.h @@ -0,0 +1,13 @@ +#pragma once + +#include "software/embedded/platform.h" + +constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = 16; +constexpr int SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = 19; +constexpr int MOTOR_DRIVER_RESET_GPIO = 12; +constexpr int DRIVER_CONTROL_ENABLE_GPIO = 22; + +// Path to the CPU thermal zone temperature file +constexpr const char CPU_TEMP_FILE_PATH[] = "/sys/class/thermal/thermal_zone0/temp"; + +constexpr Platform PLATFORM = Platform::RASP_PI; diff --git a/src/software/embedded/gpio.h b/src/software/embedded/gpio.h new file mode 100644 index 0000000000..c07e15c239 --- /dev/null +++ b/src/software/embedded/gpio.h @@ -0,0 +1,25 @@ +#pragma once + +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(GpioState, LOW, HIGH); +MAKE_ENUM(GpioDirection, INPUT, OUTPUT); + +/** + * An abstract interface for interacting with General Purpose Input/Output (GPIO) pins + */ +class Gpio +{ + public: + /** + * Set the value to the provided state + * + * @param state The state + */ + virtual void setValue(GpioState state) = 0; + + /** + * Get the current state of the gpio + */ + virtual GpioState getValue() = 0; +}; diff --git a/src/software/embedded/gpio_char_dev.cpp b/src/software/embedded/gpio_char_dev.cpp new file mode 100644 index 0000000000..772a0f6f65 --- /dev/null +++ b/src/software/embedded/gpio_char_dev.cpp @@ -0,0 +1,111 @@ +#include "software/embedded/gpio_char_dev.h" + +#include + +#include "software/logger/logger.h" + +GpioCharDev::GpioCharDev(uint32_t gpio_number, GpioDirection direction, + GpioState initial_state, std::string char_dev_path) +{ + int fd = open(char_dev_path.c_str(), O_RDONLY); + if (fd < 0) + { + LOG(FATAL) << "Could not open GPIO chip device"; + } + + struct gpiohandle_request req; + req.lineoffsets[0] = gpio_number; + req.lines = 1; + + switch (direction) + { + case GpioDirection::OUTPUT: + { + req.flags = GPIOHANDLE_REQUEST_OUTPUT; + break; + } + case GpioDirection::INPUT: + { + req.flags = GPIOHANDLE_REQUEST_INPUT; + break; + } + default: + { + LOG(FATAL) << "Invalid GPIO direction: " << direction; + } + } + + req.default_values[0] = parseGpioState(initial_state); + + int ret = ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &req); + if (ret < 0) + { + LOG(FATAL) << "Could not get GPIO line handle for GPIO " << gpio_number; + } + close(fd); + + gpio_fd = req.fd; + + LOG(DEBUG) << "GPIO " << gpio_number << " online"; +} + +void GpioCharDev::setValue(GpioState state) +{ + struct gpiohandle_data data; + data.values[0] = parseGpioState(state); + + int ret = ioctl(gpio_fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data); + if (ret < 0) + { + LOG(FATAL) << "Could not set GPIO value"; + } +} + +GpioCharDev::~GpioCharDev() +{ + close(gpio_fd); +} + +GpioState GpioCharDev::getValue() +{ + struct gpiohandle_data data; + int ret = ioctl(gpio_fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data); + + if (ret < 0) + { + LOG(FATAL) << "Could not get GPIO value"; + } + + switch (data.values[0]) + { + case 0: + { + return GpioState::LOW; + } + case 1: + { + return GpioState::HIGH; + } + } + + LOG(FATAL) << "Unable to parse GPIO value"; + return GpioState::LOW; +} + +uint8_t GpioCharDev::parseGpioState(GpioState gpio_state) +{ + switch (gpio_state) + { + case GpioState::LOW: + { + return 0; + } + case GpioState::HIGH: + { + return 1; + } + } + + LOG(FATAL) << "Invalid GPIO state: " << gpio_state; + return 0; +} diff --git a/src/software/embedded/gpio_char_dev.h b/src/software/embedded/gpio_char_dev.h new file mode 100644 index 0000000000..bb879ccbc7 --- /dev/null +++ b/src/software/embedded/gpio_char_dev.h @@ -0,0 +1,54 @@ +#pragma once + +#include + +#include "software/embedded/gpio.h" + +/** + * GPIO with the character device interface + * + * Available on Linux kernels 5.10 and later. + */ +class GpioCharDev : public Gpio +{ + public: + /** + * Communicate with GPIO pins via the new GPIO character device interface: + * https://www.kernel.org/doc/html/next/userspace-api/gpio/chardev.html + * + * @param gpio_number The gpio number + * @param direction The direction of the gpio + * @param state The initial state of the gpio + * @param char_dev_path The path to the gpio character device + */ + GpioCharDev(uint32_t gpio_number, GpioDirection direction, GpioState state, + std::string char_dev_path = "/dev/gpiochip0"); + + /** + * Destructor + */ + virtual ~GpioCharDev(); + + /** + * Set the value to the provided state + * + * @param state The state + */ + void setValue(GpioState state) override; + + /** + * Get the current state of the gpio + */ + GpioState getValue() override; + + private: + /** + * Parse the GpioState enum to a number representation + * + * @param state The state + * @return The number representation of the state + */ + uint8_t parseGpioState(GpioState state); + + int gpio_fd; // File descriptor for the gpio +}; diff --git a/src/software/jetson_nano/gpio.cpp b/src/software/embedded/gpio_sysfs.cpp similarity index 83% rename from src/software/jetson_nano/gpio.cpp rename to src/software/embedded/gpio_sysfs.cpp index 1a896f0ff7..233f11d912 100644 --- a/src/software/jetson_nano/gpio.cpp +++ b/src/software/embedded/gpio_sysfs.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/gpio.h" +#include "software/embedded/gpio_sysfs.h" #include #include @@ -10,7 +10,8 @@ #include "software/logger/logger.h" -Gpio::Gpio(std::string gpio_number, GpioDirection direction, GpioState initial_state) +GpioSysfs::GpioSysfs(const std::string& gpio_number, GpioDirection direction, + GpioState initial_state) { // Setup the provided GPIO pin gpio_number_ = gpio_number; @@ -42,11 +43,11 @@ Gpio::Gpio(std::string gpio_number, GpioDirection direction, GpioState initial_s LOG(DEBUG) << "GPIO " << gpio_number_ << " online"; } -void Gpio::setValue(GpioState state) +void GpioSysfs::setValue(GpioState state) { std::ofstream gpio_fs("/sys/class/gpio/gpio" + gpio_number_ + "/value"); - CHECK(gpio_fs.is_open()) << "Could not set GPIO pin " << gpio_number_; + CHECK(gpio_fs.is_open()) << "Could not set GPIO pin"; switch (state) { @@ -68,7 +69,7 @@ void Gpio::setValue(GpioState state) } } -GpioState Gpio::getValue() +GpioState GpioSysfs::getValue() { std::ifstream gpio_fs("/sys/class/gpio/gpio" + gpio_number_ + "/value"); std::string level; diff --git a/src/software/jetson_nano/gpio.h b/src/software/embedded/gpio_sysfs.h similarity index 61% rename from src/software/jetson_nano/gpio.h rename to src/software/embedded/gpio_sysfs.h index 7dba828c29..8d8501cd04 100644 --- a/src/software/jetson_nano/gpio.h +++ b/src/software/embedded/gpio_sysfs.h @@ -1,3 +1,5 @@ +#pragma once + #include #include #include @@ -6,12 +8,14 @@ #include #include -#include "software/util/make_enum/make_enum.hpp" - -MAKE_ENUM(GpioState, LOW, HIGH); -MAKE_ENUM(GpioDirection, INPUT, OUTPUT); +#include "software/embedded/gpio.h" -class Gpio +/** + * GPIO with the sysfs interface + * + * Deprecated in Linux kernels 4.8 and later but still available on some systems. + */ +class GpioSysfs : public Gpio { public: /* @@ -23,19 +27,20 @@ class Gpio * @param direction The direction to configure this gpio in * @param initial_state The initial GpioState of the pin */ - Gpio(std::string gpio_number, GpioDirection direction, GpioState initial_state); + GpioSysfs(const std::string& gpio_number, GpioDirection direction, + GpioState initial_state); /** * Set the value to the provided state * * @param state The state */ - void setValue(GpioState state); + void setValue(GpioState state) override; /** * Get the current state of the gpio */ - GpioState getValue(void); + GpioState getValue() override; private: std::string gpio_number_; diff --git a/src/software/embedded/linux_configs/BUILD b/src/software/embedded/linux_configs/BUILD new file mode 100644 index 0000000000..ffd0fb0cdc --- /dev/null +++ b/src/software/embedded/linux_configs/BUILD @@ -0,0 +1 @@ +package(default_visibility = ["//visibility:public"]) diff --git a/src/software/embedded/linux_configs/jetson_nano/BUILD b/src/software/embedded/linux_configs/jetson_nano/BUILD new file mode 100644 index 0000000000..26242a3daf --- /dev/null +++ b/src/software/embedded/linux_configs/jetson_nano/BUILD @@ -0,0 +1,9 @@ +package(default_visibility = ["//visibility:public"]) + +filegroup( + name = "jetson_nano_files", + data = [ + "device_tree.zip", + "extlinux.conf", + ], +) diff --git a/src/software/jetson_nano/linux_configs/device_tree.zip b/src/software/embedded/linux_configs/jetson_nano/device_tree.zip similarity index 100% rename from src/software/jetson_nano/linux_configs/device_tree.zip rename to src/software/embedded/linux_configs/jetson_nano/device_tree.zip diff --git a/src/software/jetson_nano/linux_configs/extlinux.conf b/src/software/embedded/linux_configs/jetson_nano/extlinux.conf similarity index 100% rename from src/software/jetson_nano/linux_configs/extlinux.conf rename to src/software/embedded/linux_configs/jetson_nano/extlinux.conf diff --git a/src/software/embedded/linux_configs/pi/BUILD b/src/software/embedded/linux_configs/pi/BUILD new file mode 100644 index 0000000000..e3c4156896 --- /dev/null +++ b/src/software/embedded/linux_configs/pi/BUILD @@ -0,0 +1,9 @@ +package(default_visibility = ["//visibility:public"]) + +filegroup( + name = "pi_files", + data = [ + "config.txt", + "setup.zip", + ], +) diff --git a/src/software/embedded/linux_configs/pi/config.txt b/src/software/embedded/linux_configs/pi/config.txt new file mode 100755 index 0000000000..74b3584339 --- /dev/null +++ b/src/software/embedded/linux_configs/pi/config.txt @@ -0,0 +1,50 @@ +# For more options and information see +# http://rptl.io/configtxt +# Some settings may impact device functionality. See link above for details + +# Uncomment some or all of these to enable the optional hardware interfaces +#dtparam=i2c_arm=on +#dtparam=i2s=on +dtparam=spi=on + +# Enable audio (loads snd_bcm2835) +dtparam=audio=on + +# Additional overlays and parameters are documented +# /boot/firmware/overlays/README + +# Automatically load overlays for detected cameras +camera_auto_detect=1 + +# Automatically load overlays for detected DSI displays +display_auto_detect=1 + +# Automatically load initramfs files, if found +auto_initramfs=1 + +# Enable DRM VC4 V3D driver +dtoverlay=vc4-kms-v3d +max_framebuffers=2 + +# Don't have the firmware create an initial video= setting in cmdline.txt. +# Use the kernel's default instead. +disable_fw_kms_setup=1 + +# Run in 64-bit mode +arm_64bit=1 + +# Disable compensation for displays with overscan +disable_overscan=1 + +# Run as fast as firmware / board allows +arm_boost=1 + +[cm4] +# Enable host mode on the 2711 built-in XHCI USB controller. +# This line should be removed if the legacy DWC2 controller is required +# (e.g. for USB device mode) or if USB support is not required. +otg_mode=1 + +[all] +dtparam=pciex1 +dtoverlay=spi0-5cs diff --git a/src/software/embedded/linux_configs/pi/setup.zip b/src/software/embedded/linux_configs/pi/setup.zip new file mode 100644 index 0000000000..d5b3542c1e Binary files /dev/null and b/src/software/embedded/linux_configs/pi/setup.zip differ diff --git a/src/software/embedded/linux_configs/systemd/10-persistent-wifi5-net.link b/src/software/embedded/linux_configs/systemd/10-persistent-wifi5-net.link new file mode 100644 index 0000000000..0a5cfb23ef --- /dev/null +++ b/src/software/embedded/linux_configs/systemd/10-persistent-wifi5-net.link @@ -0,0 +1,5 @@ +[Match] +Driver=brcmfmac + +[Link] +Name=tbotswifi5 diff --git a/src/software/embedded/linux_configs/systemd/10-persistent-wifi6-net.link b/src/software/embedded/linux_configs/systemd/10-persistent-wifi6-net.link new file mode 100644 index 0000000000..4bbcbd3bfa --- /dev/null +++ b/src/software/embedded/linux_configs/systemd/10-persistent-wifi6-net.link @@ -0,0 +1,5 @@ +[Match] +Driver=iwlwifi + +[Link] +Name=tbotswifi6 diff --git a/src/software/jetson_nano/linux_configs/systemd/BUILD b/src/software/embedded/linux_configs/systemd/BUILD similarity index 88% rename from src/software/jetson_nano/linux_configs/systemd/BUILD rename to src/software/embedded/linux_configs/systemd/BUILD index dc980b5a1d..f55e9ddb64 100644 --- a/src/software/jetson_nano/linux_configs/systemd/BUILD +++ b/src/software/embedded/linux_configs/systemd/BUILD @@ -4,5 +4,6 @@ filegroup( name = "systemd_files", data = glob([ "*service", + "*.link", ]), ) diff --git a/src/software/jetson_nano/linux_configs/systemd/thunderloop.service b/src/software/embedded/linux_configs/systemd/thunderloop.service similarity index 100% rename from src/software/jetson_nano/linux_configs/systemd/thunderloop.service rename to src/software/embedded/linux_configs/systemd/thunderloop.service diff --git a/src/software/embedded/platform.h b/src/software/embedded/platform.h new file mode 100644 index 0000000000..d9c9e79d96 --- /dev/null +++ b/src/software/embedded/platform.h @@ -0,0 +1,5 @@ +#pragma once + +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(Platform, RASP_PI, JETSON_NANO); diff --git a/src/software/jetson_nano/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp similarity index 99% rename from src/software/jetson_nano/primitive_executor.cpp rename to src/software/embedded/primitive_executor.cpp index 2f59490cb2..ae91e85076 100644 --- a/src/software/jetson_nano/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/primitive_executor.h" +#include "software/embedded/primitive_executor.h" #include "proto/message_translation/tbots_geometry.h" #include "proto/message_translation/tbots_protobuf.h" diff --git a/src/software/jetson_nano/primitive_executor.h b/src/software/embedded/primitive_executor.h similarity index 100% rename from src/software/jetson_nano/primitive_executor.h rename to src/software/embedded/primitive_executor.h diff --git a/src/software/jetson_nano/redis/BUILD b/src/software/embedded/redis/BUILD similarity index 100% rename from src/software/jetson_nano/redis/BUILD rename to src/software/embedded/redis/BUILD diff --git a/src/software/jetson_nano/redis/redis_client.cpp b/src/software/embedded/redis/redis_client.cpp similarity index 98% rename from src/software/jetson_nano/redis/redis_client.cpp rename to src/software/embedded/redis/redis_client.cpp index 13b6a04eab..8c43e95fcd 100644 --- a/src/software/jetson_nano/redis/redis_client.cpp +++ b/src/software/embedded/redis/redis_client.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/redis/redis_client.h" +#include "software/embedded/redis/redis_client.h" RedisClient::RedisClient(std::string host, size_t port) diff --git a/src/software/jetson_nano/redis/redis_client.h b/src/software/embedded/redis/redis_client.h similarity index 100% rename from src/software/jetson_nano/redis/redis_client.h rename to src/software/embedded/redis/redis_client.h diff --git a/src/software/jetson_nano/redis/redis_client_test.cpp b/src/software/embedded/redis/redis_client_test.cpp similarity index 98% rename from src/software/jetson_nano/redis/redis_client_test.cpp rename to src/software/embedded/redis/redis_client_test.cpp index b4b072f34a..065ad158c7 100644 --- a/src/software/jetson_nano/redis/redis_client_test.cpp +++ b/src/software/embedded/redis/redis_client_test.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/redis/redis_client.h" +#include "software/embedded/redis/redis_client.h" #include #include diff --git a/src/software/jetson_nano/services/BUILD b/src/software/embedded/services/BUILD similarity index 80% rename from src/software/jetson_nano/services/BUILD rename to src/software/embedded/services/BUILD index e2ca376da2..d1f6f19aa7 100644 --- a/src/software/jetson_nano/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -4,10 +4,16 @@ cc_library( name = "motor", srcs = ["motor.cpp"], hdrs = ["motor.h"], + defines = + select({ + "//software/embedded:build_nano": ["NANO"], + "//software/embedded:build_pi": ["PI"], + }), deps = [ "//proto:tbots_cc_proto", "//shared:robot_constants", - "//software/jetson_nano:gpio", + "//software/embedded:gpio", + "//software/embedded/constants", "//software/logger", "//software/physics:euclidean_to_wheel", "//software/util/scoped_timespec_timer", @@ -43,7 +49,7 @@ cc_binary( "//proto/message_translation:tbots_geometry", "//proto/primitive:primitive_msg_factory", "//shared:robot_constants", - "//software/jetson_nano:primitive_executor", + "//software/embedded:primitive_executor", "//software/logger", "@trinamic", ], diff --git a/src/software/jetson_nano/services/motor.cpp b/src/software/embedded/services/motor.cpp similarity index 96% rename from src/software/jetson_nano/services/motor.cpp rename to src/software/embedded/services/motor.cpp index b1c095a1a1..c8e06e9910 100644 --- a/src/software/jetson_nano/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/services/motor.h" +#include "software/embedded/services/motor.h" #include #include @@ -21,7 +21,6 @@ #include #include "proto/tbots_software_msgs.pb.h" -#include "shared/constants.h" #include "software/logger/logger.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" @@ -42,12 +41,6 @@ static const uint32_t SPI_MODE = 0x3u; static const uint32_t NUM_RETRIES_SPI = 3; static const uint32_t TMC_CMD_MSG_SIZE = 5; - -static const char* SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO = "51"; -static const char* SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO = "76"; -static const char* MOTOR_DRIVER_RESET_GPIO = "168"; -static const char* DRIVER_CONTROL_ENABLE_GPIO = "194"; - static double RUNAWAY_PROTECTION_THRESHOLD_MPS = 2.00; static int DRIBBLER_ACCELERATION_THRESHOLD_RPM_PER_S_2 = 10000; @@ -74,13 +67,14 @@ extern "C" MotorService::MotorService(const RobotConstants_t& robot_constants, int control_loop_frequency_hz) - : spi_demux_select_0_(SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO, GpioDirection::OUTPUT, - GpioState::LOW), - spi_demux_select_1_(SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO, GpioDirection::OUTPUT, - GpioState::LOW), - driver_control_enable_gpio_(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, - GpioState::HIGH), - reset_gpio_(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH), + : spi_demux_select_0_(setupGpio(SPI_CS_DRIVER_TO_CONTROLLER_MUX_0_GPIO, + GpioDirection::OUTPUT, GpioState::LOW)), + spi_demux_select_1_(setupGpio(SPI_CS_DRIVER_TO_CONTROLLER_MUX_1_GPIO, + GpioDirection::OUTPUT, GpioState::LOW)), + driver_control_enable_gpio_( + setupGpio(DRIVER_CONTROL_ENABLE_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), + reset_gpio_( + setupGpio(MOTOR_DRIVER_RESET_GPIO, GpioDirection::OUTPUT, GpioState::HIGH)), robot_constants_(robot_constants), euclidean_to_four_wheel_(robot_constants), motor_fault_detector_(0), @@ -168,10 +162,10 @@ void MotorService::setup() prev_wheel_velocities_ = {0.0, 0.0, 0.0, 0.0}; // Clear faults by resetting all the chips on the motor board - reset_gpio_.setValue(GpioState::LOW); + reset_gpio_->setValue(GpioState::LOW); usleep(MICROSECONDS_PER_MILLISECOND * 100); - reset_gpio_.setValue(GpioState::HIGH); + reset_gpio_->setValue(GpioState::HIGH); usleep(MICROSECONDS_PER_MILLISECOND * 100); for (uint8_t motor = 0; motor < NUM_MOTORS; ++motor) @@ -485,28 +479,28 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor prev_wheel_velocities_[FRONT_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_.setValue(GpioState::LOW); + driver_control_enable_gpio_->setValue(GpioState::LOW); LOG(FATAL) << "Front right motor runaway"; } else if (std::abs(current_wheel_velocities[FRONT_LEFT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[FRONT_LEFT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_.setValue(GpioState::LOW); + driver_control_enable_gpio_->setValue(GpioState::LOW); LOG(FATAL) << "Front left motor runaway"; } else if (std::abs(current_wheel_velocities[BACK_LEFT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[BACK_LEFT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_.setValue(GpioState::LOW); + driver_control_enable_gpio_->setValue(GpioState::LOW); LOG(FATAL) << "Back left motor runaway"; } else if (std::abs(current_wheel_velocities[BACK_RIGHT_WHEEL_SPACE_INDEX] - prev_wheel_velocities_[BACK_RIGHT_WHEEL_SPACE_INDEX]) > RUNAWAY_PROTECTION_THRESHOLD_MPS) { - driver_control_enable_gpio_.setValue(GpioState::LOW); + driver_control_enable_gpio_->setValue(GpioState::LOW); LOG(FATAL) << "Back right motor runaway"; } @@ -700,16 +694,16 @@ void MotorService::readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, uint8_t MotorService::tmc4671ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) { - spi_demux_select_0_.setValue(GpioState::HIGH); - spi_demux_select_1_.setValue(GpioState::LOW); + spi_demux_select_0_->setValue(GpioState::HIGH); + spi_demux_select_1_->setValue(GpioState::LOW); return readWriteByte(motor, data, last_transfer, TMC4671_SPI_SPEED); } int32_t MotorService::tmc4671ReadThenWriteValue(uint8_t motor, uint8_t read_addr, uint8_t write_addr, int32_t write_data) { - spi_demux_select_0_.setValue(GpioState::HIGH); - spi_demux_select_1_.setValue(GpioState::LOW); + spi_demux_select_0_->setValue(GpioState::HIGH); + spi_demux_select_1_->setValue(GpioState::LOW); // ensure tx_ and rx_ are cleared memset(read_tx_, 0, 5); memset(write_tx_, 0, 5); @@ -748,8 +742,8 @@ int32_t MotorService::tmc4671ReadThenWriteValue(uint8_t motor, uint8_t read_addr uint8_t MotorService::tmc6100ReadWriteByte(uint8_t motor, uint8_t data, uint8_t last_transfer) { - spi_demux_select_0_.setValue(GpioState::LOW); - spi_demux_select_1_.setValue(GpioState::HIGH); + spi_demux_select_0_->setValue(GpioState::LOW); + spi_demux_select_1_->setValue(GpioState::HIGH); return readWriteByte(motor, data, last_transfer, TMC6100_SPI_SPEED); } @@ -836,7 +830,7 @@ void MotorService::writeToDriverOrDieTrying(uint8_t motor, uint8_t address, int3 // If we get here, we have failed to write to the driver. We reset // the chip to clear any bad values we just wrote and crash so everything stops. - reset_gpio_.setValue(GpioState::LOW); + reset_gpio_->setValue(GpioState::LOW); CHECK(read_value == value) << "Couldn't write " << value << " to the TMC6100 at address " << address << " at address " << static_cast(address) @@ -876,7 +870,7 @@ void MotorService::writeToControllerOrDieTrying(uint8_t motor, uint8_t address, // If we get here, we have failed to write to the controller. We reset // the chip to clear any bad values we just wrote and crash so everything stops. - reset_gpio_.setValue(GpioState::LOW); + reset_gpio_->setValue(GpioState::LOW); CHECK(read_value == value) << "Couldn't write " << value << " to the TMC4671 at address " << address << " at address " << static_cast(address) @@ -1201,5 +1195,5 @@ void MotorService::checkEncoderConnections() void MotorService::resetMotorBoard() { - reset_gpio_.setValue(GpioState::LOW); + reset_gpio_->setValue(GpioState::LOW); } diff --git a/src/software/jetson_nano/services/motor.h b/src/software/embedded/services/motor.h similarity index 89% rename from src/software/jetson_nano/services/motor.h rename to src/software/embedded/services/motor.h index 5b68a30dda..bb713e3807 100644 --- a/src/software/jetson_nano/services/motor.h +++ b/src/software/embedded/services/motor.h @@ -6,10 +6,23 @@ #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" +#include "shared/constants.h" #include "shared/robot_constants.h" -#include "software/jetson_nano/gpio.h" +#include "software/embedded/constants/constants.h" +#include "software/embedded/gpio.h" +#include "software/embedded/gpio_char_dev.h" +#include "software/embedded/gpio_sysfs.h" +#include "software/embedded/platform.h" #include "software/physics/euclidean_to_wheel.h" +/** + * A service that interacts with the motor. + * + * It is responsible for: + * - Converting Euclidean velocities to wheel velocities + * - Communicating with the motor + * - Detecting and handling faults + */ class MotorService { public: @@ -312,6 +325,20 @@ class MotorService double back_right_velocity_mps, double dribbler_rpm); + /** + * Helper function to setup a GPIO pin. Selects the appropriate GPIO implementation + * based on the host platform. + * + * @tparam T The representation of the GPIO number + * @param gpio_number The GPIO number (this is typically different from the hardware + * pin number) + * @param direction The direction of the GPIO pin (input or output) + * @param initial_state The initial state of the GPIO pin (high or low) + */ + template + static std::unique_ptr setupGpio(const T& gpio_number, GpioDirection direction, + GpioState initial_state); + /** * Returns true if we've detected a RESET in our cached motor faults indicators or if * we have a fault that disables drive. @@ -335,12 +362,12 @@ class MotorService bool is_initialized_ = false; // Select between driver and controller gpio - Gpio spi_demux_select_0_; - Gpio spi_demux_select_1_; + std::unique_ptr spi_demux_select_0_; + std::unique_ptr spi_demux_select_1_; // Enable driver gpio - Gpio driver_control_enable_gpio_; - Gpio reset_gpio_; + std::unique_ptr driver_control_enable_gpio_; + std::unique_ptr reset_gpio_; // Transfer Buffers for spiTransfer uint8_t tx_[5] = {0}; @@ -403,3 +430,19 @@ class MotorService static constexpr const char* MOTOR_NAMES[] = {"front_left", "back_left", "back_right", "front_right", "dribbler"}; }; + +template +std::unique_ptr MotorService::setupGpio(const T& gpio_number, + GpioDirection direction, + GpioState initial_state) +{ + if constexpr (PLATFORM == Platform::RASP_PI) + { + return std::make_unique(gpio_number, direction, initial_state, + "/dev/gpiochip4"); + } + else + { + return std::make_unique(gpio_number, direction, initial_state); + } +} diff --git a/src/software/jetson_nano/services/network/BUILD b/src/software/embedded/services/network/BUILD similarity index 100% rename from src/software/jetson_nano/services/network/BUILD rename to src/software/embedded/services/network/BUILD diff --git a/src/software/jetson_nano/services/network/network.cpp b/src/software/embedded/services/network/network.cpp similarity index 98% rename from src/software/jetson_nano/services/network/network.cpp rename to src/software/embedded/services/network/network.cpp index dfb9e92865..2dfe4ae8bb 100644 --- a/src/software/jetson_nano/services/network/network.cpp +++ b/src/software/embedded/services/network/network.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/services/network/network.h" +#include "software/embedded/services/network/network.h" NetworkService::NetworkService(const std::string& ip_address, unsigned short primitive_listener_port, diff --git a/src/software/jetson_nano/services/network/network.h b/src/software/embedded/services/network/network.h similarity index 98% rename from src/software/jetson_nano/services/network/network.h rename to src/software/embedded/services/network/network.h index df61f0806e..b136382757 100644 --- a/src/software/jetson_nano/services/network/network.h +++ b/src/software/embedded/services/network/network.h @@ -7,7 +7,7 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" #include "shared/robot_constants.h" -#include "software/jetson_nano/services/network/proto_tracker.h" +#include "software/embedded/services/network/proto_tracker.h" #include "software/networking/radio/threaded_proto_radio_listener.hpp" #include "software/networking/udp/threaded_proto_udp_listener.hpp" #include "software/networking/udp/threaded_proto_udp_sender.hpp" diff --git a/src/software/jetson_nano/services/network/proto_tracker.cpp b/src/software/embedded/services/network/proto_tracker.cpp similarity index 96% rename from src/software/jetson_nano/services/network/proto_tracker.cpp rename to src/software/embedded/services/network/proto_tracker.cpp index ee756691f8..500ae993e0 100644 --- a/src/software/jetson_nano/services/network/proto_tracker.cpp +++ b/src/software/embedded/services/network/proto_tracker.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/services/network/proto_tracker.h" +#include "software/embedded/services/network/proto_tracker.h" ProtoTracker::ProtoTracker(const std::string& type) : proto_type(type) {} diff --git a/src/software/jetson_nano/services/network/proto_tracker.h b/src/software/embedded/services/network/proto_tracker.h similarity index 100% rename from src/software/jetson_nano/services/network/proto_tracker.h rename to src/software/embedded/services/network/proto_tracker.h diff --git a/src/software/jetson_nano/services/network/proto_tracker_test.cpp b/src/software/embedded/services/network/proto_tracker_test.cpp similarity index 96% rename from src/software/jetson_nano/services/network/proto_tracker_test.cpp rename to src/software/embedded/services/network/proto_tracker_test.cpp index 77575467e4..1edf0f3ed0 100644 --- a/src/software/jetson_nano/services/network/proto_tracker_test.cpp +++ b/src/software/embedded/services/network/proto_tracker_test.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/services/network/proto_tracker.h" +#include "software/embedded/services/network/proto_tracker.h" #include diff --git a/src/software/jetson_nano/services/power.cpp b/src/software/embedded/services/power.cpp similarity index 97% rename from src/software/jetson_nano/services/power.cpp rename to src/software/embedded/services/power.cpp index 1ae2258ef1..5699bd8b9f 100644 --- a/src/software/jetson_nano/services/power.cpp +++ b/src/software/embedded/services/power.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/services/power.h" +#include "software/embedded/services/power.h" #include #include diff --git a/src/software/jetson_nano/services/power.h b/src/software/embedded/services/power.h similarity index 100% rename from src/software/jetson_nano/services/power.h rename to src/software/embedded/services/power.h diff --git a/src/software/jetson_nano/services/robot_auto_test.cpp b/src/software/embedded/services/robot_auto_test.cpp similarity index 79% rename from src/software/jetson_nano/services/robot_auto_test.cpp rename to src/software/embedded/services/robot_auto_test.cpp index b940d698be..5307b16815 100644 --- a/src/software/jetson_nano/services/robot_auto_test.cpp +++ b/src/software/embedded/services/robot_auto_test.cpp @@ -2,9 +2,9 @@ #include "proto/primitive/primitive_msg_factory.h" #include "shared/2021_robot_constants.h" #include "shared/constants.h" -#include "software/jetson_nano/primitive_executor.h" -#include "software/jetson_nano/services/motor.h" -#include "software/jetson_nano/services/power.h" +#include "software/embedded/primitive_executor.h" +#include "software/embedded/services/motor.h" +#include "software/embedded/services/power.h" #include "software/logger/network_logger.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" @@ -34,7 +34,8 @@ std::string runtime_dir = "/tmp/tbots/yellow_test"; int main(int argc, char **argv) { - LoggerSingleton::initializeLogger(runtime_dir, nullptr); + LoggerSingleton::initializeLogger(runtime_dir, nullptr, false); + LOG(INFO) << "Running on the Jetson Nano!"; motor_service_ = @@ -43,10 +44,12 @@ int main(int argc, char **argv) // Testing Motor board SPI transfer for (uint8_t chip_select : CHIP_SELECT) { + LOG(INFO) << "Checking motor: " << int(chip_select); + // Check driver fault if (!motor_service_->checkDriverFault(chip_select).drive_enabled) { - LOG(FATAL) << "Detected Motor Fault"; + LOG(WARNING) << "Detected Motor Fault"; } motor_service_->writeIntToTMC4671(chip_select, TMC4671_CHIPINFO_ADDR, @@ -61,10 +64,12 @@ int main(int argc, char **argv) } else { - LOG(FATAL) << "SPI Transfer not successful"; + LOG(WARNING) << "SPI Transfer not successful"; } } + motor_service_->resetMotorBoard(); + // Testing Power board UART transfer try { @@ -78,23 +83,25 @@ int main(int argc, char **argv) if (abs(power_status.battery_voltage()) < THRESHOLD) { - LOG(FATAL) << "Battery voltage is zero"; + LOG(WARNING) << "Battery voltage is zero"; } else if (abs(power_status.capacitor_voltage()) < THRESHOLD) { - LOG(FATAL) << "Capacitor voltage is zero"; + LOG(WARNING) << "Capacitor voltage is zero"; } else if (abs(power_status.current_draw()) < THRESHOLD) { - LOG(FATAL) << "Current draw is zero"; + LOG(WARNING) << "Current draw is zero"; } else if (power_status.sequence_num() == 0) { - LOG(FATAL) << "Sequence number is zero"; + LOG(WARNING) << "Sequence number is zero"; } } catch (std::runtime_error &e) { - LOG(FATAL) << "Unable to communicate with the power board"; + LOG(WARNING) << "Unable to communicate with the power board"; } + + return 0; } diff --git a/src/software/jetson_nano/setup_nano.sh b/src/software/embedded/setup_robot_software_deps.sh similarity index 81% rename from src/software/jetson_nano/setup_nano.sh rename to src/software/embedded/setup_robot_software_deps.sh index ab58fa9b00..4751d83682 100755 --- a/src/software/jetson_nano/setup_nano.sh +++ b/src/software/embedded/setup_robot_software_deps.sh @@ -4,17 +4,12 @@ set -ex host_software_packages=( redis - libjpeg8-dev - libjpeg-dev device-tree-compiler - zlib1g-dev curl ) -sudo apt-get update -sudo add-apt-repository -y ppa:deadsnakes/ppa - # Install packages +sudo apt-get update sudo apt-get install "${host_software_packages[@]}" -y # Install platformio udev rules diff --git a/src/software/jetson_nano/thunderloop.cpp b/src/software/embedded/thunderloop.cpp similarity index 94% rename from src/software/jetson_nano/thunderloop.cpp rename to src/software/embedded/thunderloop.cpp index e0863821c0..bc6d685531 100644 --- a/src/software/jetson_nano/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -1,4 +1,4 @@ -#include "software/jetson_nano/thunderloop.h" +#include "software/embedded/thunderloop.h" #include #include @@ -9,8 +9,8 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/2021_robot_constants.h" #include "shared/constants.h" -#include "software/jetson_nano/primitive_executor.h" -#include "software/jetson_nano/services/motor.h" +#include "software/embedded/primitive_executor.h" +#include "software/embedded/services/motor.h" #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/tracy/tracy_constants.h" @@ -86,6 +86,8 @@ Thunderloop::Thunderloop(const RobotConstants_t& robot_constants, bool enable_lo primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, TeamColour::YELLOW, robot_id_) { + waitForNetworkUp(); + g3::overrideSetupSignals({}); NetworkLoggerSingleton::initializeLogger(channel_id_, network_interface_, robot_id_, enable_log_merging); @@ -134,7 +136,7 @@ Thunderloop::~Thunderloop() {} /* * Run the main robot loop! */ -[[noreturn]] void Thunderloop::runLoop() +void Thunderloop::runLoop() { // Timing struct timespec next_shot; @@ -473,3 +475,30 @@ void Thunderloop::updateErrorCodes() TbotsProto::ErrorCode::UNSTABLE_POWER_SUPPLY); } } + +void Thunderloop::waitForNetworkUp() +{ + ThreadedUdpSender network_test( + std::string(ROBOT_MULTICAST_CHANNELS.at(channel_id_)) + "%" + network_interface_, + NETWORK_COMM_TEST_PORT, true); + + // Send an empty packet on the specific network interface to + // ensure wifi is connected. Keeps trying until successful + while (true) + { + try + { + network_test.sendString(""); + break; + } + catch (std::exception& e) + { + // Resend the message after a delay + LOG(WARNING) << "Thunderloop cannot connect to network!" + << "Waiting for connection..."; + sleep(PING_RETRY_DELAY_S); + } + } + + LOG(INFO) << "Thunderloop connected to network!"; +} diff --git a/src/software/jetson_nano/thunderloop.h b/src/software/embedded/thunderloop.h similarity index 91% rename from src/software/jetson_nano/thunderloop.h rename to src/software/embedded/thunderloop.h index b266d86c1c..33fadb2577 100644 --- a/src/software/jetson_nano/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -9,11 +9,11 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/2021_robot_constants.h" #include "shared/constants.h" -#include "software/jetson_nano/primitive_executor.h" -#include "software/jetson_nano/redis/redis_client.h" -#include "software/jetson_nano/services/motor.h" -#include "software/jetson_nano/services/network/network.h" -#include "software/jetson_nano/services/power.h" +#include "software/embedded/primitive_executor.h" +#include "software/embedded/redis/redis_client.h" +#include "software/embedded/services/motor.h" +#include "software/embedded/services/network/network.h" +#include "software/embedded/services/power.h" #include "software/logger/logger.h" #include "software/world/robot_state.h" @@ -55,7 +55,7 @@ class Thunderloop ~Thunderloop(); - void runLoop(); + [[noreturn]] void runLoop(); // Services std::unique_ptr motor_service_; @@ -99,6 +99,11 @@ class Thunderloop */ void updateErrorCodes(); + /** + * Wait for networking communication to be established. This function is blocking. + */ + void waitForNetworkUp(); + // Input Msg Buffers TbotsProto::PrimitiveSet primitive_set_; @@ -136,8 +141,9 @@ class Thunderloop // 500 millisecond timeout on receiving primitives before we stop the robots const double PACKET_TIMEOUT_NS = 500.0 * NANOSECONDS_PER_MILLISECOND; - // Path to the CPU thermal zone temperature file - const std::string CPU_TEMP_FILE_PATH = "/sys/class/thermal/thermal_zone1/temp"; + // Timeout after a failed ping request + const int PING_RETRY_DELAY_S = 1; + const std::string PATH_TO_RINGBUFFER_LOG = "/var/log/dmesg"; std::ifstream log_file = std::ifstream(PATH_TO_RINGBUFFER_LOG); diff --git a/src/software/jetson_nano/thunderloop_main.cpp b/src/software/embedded/thunderloop_main.cpp similarity index 98% rename from src/software/jetson_nano/thunderloop_main.cpp rename to src/software/embedded/thunderloop_main.cpp index b274c5ae7e..7218380629 100644 --- a/src/software/jetson_nano/thunderloop_main.cpp +++ b/src/software/embedded/thunderloop_main.cpp @@ -13,7 +13,7 @@ #include "proto/tbots_software_msgs.pb.h" #include "shared/2021_robot_constants.h" #include "shared/constants.h" -#include "software/jetson_nano/thunderloop.h" +#include "software/embedded/thunderloop.h" #include "software/logger/network_logger.h" #include "software/world/robot_state.h" diff --git a/src/software/estop/arduino_util.cpp b/src/software/estop/arduino_util.cpp index 0a3a6bb651..3ee6220e7e 100644 --- a/src/software/estop/arduino_util.cpp +++ b/src/software/estop/arduino_util.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include "shared/constants.h" @@ -109,7 +110,7 @@ std::vector ArduinoUtil::getSerialDevices() std::optional ArduinoUtil::readFileLine(boost::filesystem::path path) { - boost::filesystem::ifstream f(path.c_str()); + std::ifstream f(path.c_str()); std::string res; if (f.is_open()) { diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index 49e43b3254..1bb966aa21 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -26,14 +26,12 @@ # # move_tactic = MoveTactic() # move_tactic.destination.CopyFrom(rob_pos_p) -# move_tactic.final_speed = 0.0 # move_tactic.dribbler_mode = DribblerMode.OFF # move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) # move_tactic.ball_collision_type = BallCollisionType.AVOID # move_tactic.auto_chip_or_kick.CopyFrom(AutoChipOrKick(autokick_speed_m_per_s=0.0)) # move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT # move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE -# move_tactic.target_spin_rev_per_s = 0.0 # # # setup world state # initial_worldstate = create_world_state( @@ -95,7 +93,6 @@ def test_basic_rotation(field_test_runner): for angle in test_angles: move_tactic = MoveTactic() move_tactic.destination.CopyFrom(rob_pos_p) - move_tactic.final_speed = 0.0 move_tactic.dribbler_mode = DribblerMode.OFF move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) move_tactic.ball_collision_type = BallCollisionType.AVOID @@ -104,7 +101,6 @@ def test_basic_rotation(field_test_runner): ) move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE - move_tactic.target_spin_rev_per_s = 0.0 # Setup Tactic params = AssignedTacticPlayControlParams() @@ -117,11 +113,11 @@ def test_basic_rotation(field_test_runner): eventually_validation_sequence_set=[[]], test_timeout_s=5, ) - # Send a stop tactic after the test finishes - stop_tactic = StopTactic() + # Send a halt tactic after the test finishes + halt_tactic = HaltTactic() params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].stop.CopyFrom(stop_tactic) - # send the stop tactic + params.assigned_tactics[id].stop.CopyFrom(halt_tactic) + # send the halt tactic field_test_runner.set_tactics(params, True) # validate by eye @@ -153,47 +149,39 @@ def test_one_robots_square(field_test_runner): tactic_0 = MoveTactic( destination=point1, - final_speed=0.0, dribbler_mode=DribblerMode.OFF, final_orientation=Angle(radians=-math.pi / 2), ball_collision_type=BallCollisionType.AVOID, auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, - target_spin_rev_per_s=0.0, ) tactic_1 = MoveTactic( destination=point2, - final_speed=0.0, dribbler_mode=DribblerMode.OFF, final_orientation=Angle(radians=-math.pi / 2), ball_collision_type=BallCollisionType.AVOID, auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, - target_spin_rev_per_s=0.0, ) tactic_2 = MoveTactic( destination=point3, - final_speed=0.0, dribbler_mode=DribblerMode.OFF, final_orientation=Angle(radians=-math.pi / 2), ball_collision_type=BallCollisionType.AVOID, auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, - target_spin_rev_per_s=0.0, ) tactic_3 = MoveTactic( destination=point4, - final_speed=0.0, dribbler_mode=DribblerMode.OFF, final_orientation=Angle(radians=-math.pi / 2), ball_collision_type=BallCollisionType.AVOID, auto_chip_or_kick=AutoChipOrKick(autokick_speed_m_per_s=0.0), max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, - target_spin_rev_per_s=0.0, ) tactics = [tactic_0, tactic_1, tactic_2, tactic_3] @@ -209,10 +197,10 @@ def test_one_robots_square(field_test_runner): test_timeout_s=4, ) - # Send a stop tactic after the test finishes - stop_tactic = StopTactic() + # Send a halt tactic after the test finishes + halt_tactic = HaltTactic() params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].stop.CopyFrom(stop_tactic) + params.assigned_tactics[id].stop.CopyFrom(halt_tactic) if __name__ == "__main__": diff --git a/src/software/field_tests/passing_field_test.py b/src/software/field_tests/passing_field_test.py index eff8acafc1..7240754338 100644 --- a/src/software/field_tests/passing_field_test.py +++ b/src/software/field_tests/passing_field_test.py @@ -96,12 +96,11 @@ def test_passing(field_test_runner): test_timeout_s=5, ) - # Send a stop tactic after the test finishes - stop_tactic = StopTactic() + # Send a halt tactic after the test finishes + halt_tactic = HaltTactic() params = AssignedTacticPlayControlParams() - params.assigned_tactics[passer_robot_id].stop.CopyFrom(stop_tactic) - params.assigned_tactics[receiver_robot_id].stop.CopyFrom(stop_tactic) - # send the stop tactic + params.assigned_tactics[passer_robot_id].stop.CopyFrom(halt_tactic) + params.assigned_tactics[receiver_robot_id].stop.CopyFrom(halt_tactic) field_test_runner.set_tactics(params, True) diff --git a/src/software/field_tests/pivot_kick_field_test.py b/src/software/field_tests/pivot_kick_field_test.py index 432171ccfb..4edeaa2167 100644 --- a/src/software/field_tests/pivot_kick_field_test.py +++ b/src/software/field_tests/pivot_kick_field_test.py @@ -38,10 +38,10 @@ def test_pivot_kick(field_test_runner): eventually_validation_sequence_set=[[]], test_timeout_s=15, ) - # Send a stop tactic after the test finishes - stop_tactic = StopTactic() + # Send a halt tactic after the test finishes + halt_tactic = HaltTactic() params = AssignedTacticPlayControlParams() - params.assigned_tactics[id].stop.CopyFrom(stop_tactic) + params.assigned_tactics[id].stop.CopyFrom(halt_tactic) if __name__ == "__main__": diff --git a/src/software/jetson_nano/ansible/playbooks/setup_nano.yml b/src/software/jetson_nano/ansible/playbooks/setup_nano.yml deleted file mode 100644 index d05f20ff74..0000000000 --- a/src/software/jetson_nano/ansible/playbooks/setup_nano.yml +++ /dev/null @@ -1,242 +0,0 @@ ---- - -- name: Setting up the jetson Nano - hosts: THUNDERBOTS_HOSTS - - tasks: - - - name: Run basic setup - tags: dependencies - block: - - name: check internet connection - uri: - url: "https://google.com" - timeout: 30 - status_code: 200 - - - name: enable passwordless sudo for rsync - become: true - become_method: sudo - lineinfile: - path: /etc/sudoers - state: present - insertafter: EOF - line: '{{ ansible_user }} ALL=NOPASSWD:/usr/bin/rsync' - - - name: Run Jetson Clocks - become: true - become_method: sudo - shell: /usr/bin/jetson_clocks - - - name: Sync Setup Script - become: true - become_method: sudo - tags: - - sync - ansible.posix.synchronize: - src: ../../setup_nano.sh - dest: ~/ - recursive: yes - copy_links: yes - - # Output of each nano streamed to that Nano's /tmp/setup.log - - name: Running the setup script, this will take a while - become_method: sudo - become: true - command: '/home/{{ ansible_user }}/setup_nano.sh >& /tmp/setup.log' - register: result - args: - chdir: ~/ - - - name: success msg - debug: - msg: - - "[Robot ID = {{ inventory_hostname }}]" - - "Setup Script run successfully" - - # UART setup - - name: Stop nvgetty - become: true - become_method: sudo - command: 'systemctl stop nvgetty' - - - name: Disable nvgetty - become: true - become_method: sudo - command: 'systemctl disable nvgetty' - - - name: Setup udevadm trigger - become: true - become_method: sudo - command: 'udevadm trigger' - - - name: Add user robot to dialout - become: true - become_method: sudo - command: 'adduser robot dialout' - - # Device tree setup - - name: Sync Device Tree - become: true - become_method: sudo - tags: - - sync - - device_tree - ansible.posix.synchronize: - src: ../../../linux_configs/device_tree.zip - dest: ~/ - recursive: yes - copy_links: yes - - - name: Sync extlinux conf - become: true - become_method: sudo - tags: - - sync - - device_tree - ansible.posix.synchronize: - src: ../../../linux_configs/extlinux.conf - dest: ~/ - recursive: yes - copy_links: yes - - - name: Unzip device tree - tags: - - sync - - device_tree - command: 'unzip -o ~/device_tree.zip' - register: result - args: - chdir: ~/ - - - name: Compile device tree - tags: - - sync - - device_tree - shell: 'dtc -q -I dts -O dtb ~/device_tree.dts > device_tree.dtb' - register: result - args: - chdir: ~/ - - - - name: Move compiled device tree binary to boot path - become_method: sudo - become: true - tags: - - sync - - device_tree - shell: 'mv /home/robot/device_tree.dtb /boot/kernel_tegra210-p3448-0000-p3449-0000-b00-user-custom.dtb' - register: result - args: - chdir: ~/ - - - name: Move extlinux file - become_method: sudo - become: true - tags: - - sync - - device_tree - shell: 'mv /home/robot/extlinux.conf /boot/extlinux/extlinux.conf' - register: result - args: - chdir: ~/ - - - - name: Run systemd setup - tags: systemd - block: - - name: Delete everything on ~/ - file: - state: absent - path: /home/robot/thunderbots_binaries - become_method: sudo - become: true - register: result - - - name: Sync Binaries - with_items: - # bazel makes its own zip files, we append another zip to deconflict - - ../../thunderloop_main - loop_control: - loop_var: binary_path - ansible.posix.synchronize: - src: "{{ binary_path }}" - dest: ~/thunderbots_binaries/ - recursive: yes - copy_links: yes - tags: - - syncBinaries - - - name: Unzipping python binaries - command: 'unzip ~/thunderbots_binaries/*.zip' - register: result - args: - chdir: ~/thunderbots_binaries - - - name: Sync all systemd files - become_method: sudo - become: true - tags: - - syncSystemdFiles - register: res - ansible.posix.synchronize: - src: ../../linux_configs/systemd/ - dest: /etc/systemd/system - copy_links: yes - - - name: Remove announcements - become_method: sudo - become: true - tags: - - syncSystemdFiles - file: - path: "/etc/systemd/system/{{ service_name }}.service" - state: absent - with_items: ["wifi_announcement", "ethernet_announcement"] - loop_control: - loop_var: service_name - - # NOTE: "Enabling systems" means they will start on boot - - name: Enable system services - become: true - become_method: sudo - with_items: - - thunderbots - - thunderloop - loop_control: - loop_var: service_name - ansible.builtin.systemd: - name: "{{ service_name }}" - enabled: yes - masked: no - daemon_reload: yes - tags: - - enableSystemd - - - name: Enable WiFi waiter service - become: true - become_method: sudo - service: name=NetworkManager-wait-online enabled=yes - - - name: Reboot - become: true - become_user: root - become_method: sudo - register: res - tags: - - reboot - - device_tree - reboot: - msg: "Reboot initiated by Ansible" - connect_timeout: 20 - reboot_timeout: 1200 - pre_reboot_delay: 0 - post_reboot_delay: 10 - test_command: whoami - - - name: Success msg - debug: - msg: - - "[Robot ID = {{ inventory_hostname }}]" - - "Systemd Setup run successfully" - diff --git a/src/software/jetson_nano/ansible/requirements.in b/src/software/jetson_nano/ansible/requirements.in deleted file mode 100644 index 72b3f6f58c..0000000000 --- a/src/software/jetson_nano/ansible/requirements.in +++ /dev/null @@ -1 +0,0 @@ -ansible==5.3.0 diff --git a/src/software/jetson_nano/linux_configs/BUILD b/src/software/jetson_nano/linux_configs/BUILD deleted file mode 100644 index f814f5213c..0000000000 --- a/src/software/jetson_nano/linux_configs/BUILD +++ /dev/null @@ -1,11 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -filegroup( - name = "device_tree", - data = ["device_tree.zip"], -) - -filegroup( - name = "extlinux", - data = ["extlinux.conf"], -) diff --git a/src/software/jetson_nano/linux_configs/systemd/thunderbots.service b/src/software/jetson_nano/linux_configs/systemd/thunderbots.service deleted file mode 100644 index f430346a07..0000000000 --- a/src/software/jetson_nano/linux_configs/systemd/thunderbots.service +++ /dev/null @@ -1,16 +0,0 @@ -[Unit] -Description=thunderbots - -[Service] -# The dummy program will exit -Type=oneshot - -# Execute a dummy program -ExecStart=/bin/true - -# This service shall be considered active after start -RemainAfterExit=yes - -[Install] -# Components of this application should be started at boot time -WantedBy=multi-user.target diff --git a/src/software/logger/coloured_cout_sink.cpp b/src/software/logger/coloured_cout_sink.cpp index aae1817442..aebc47a5e0 100644 --- a/src/software/logger/coloured_cout_sink.cpp +++ b/src/software/logger/coloured_cout_sink.cpp @@ -2,8 +2,10 @@ #include "software/logger/custom_logging_levels.h" -ColouredCoutSink::ColouredCoutSink(bool print_detailed) - : print_detailed(print_detailed), log_merger(LogMerger()) +ColouredCoutSink::ColouredCoutSink(bool print_detailed, bool reduce_repetition) + : print_detailed(print_detailed), + reduce_repetition(reduce_repetition), + log_merger(LogMerger()) { } @@ -54,9 +56,16 @@ void ColouredCoutSink::resetColour() void ColouredCoutSink::displayColouredLog(g3::LogMessageMover log_entry) { g3::LogMessage new_log = log_entry.get(); - for (g3::LogMessage log : log_merger.log(new_log)) + if (reduce_repetition) { - displaySingleLog(log); + for (g3::LogMessage log : log_merger.log(new_log)) + { + displaySingleLog(log); + } + } + else + { + displaySingleLog(new_log); } } diff --git a/src/software/logger/coloured_cout_sink.h b/src/software/logger/coloured_cout_sink.h index f12ea2d5bc..238dc2de3c 100644 --- a/src/software/logger/coloured_cout_sink.h +++ b/src/software/logger/coloured_cout_sink.h @@ -18,7 +18,7 @@ class ColouredCoutSink * @param print_detailed If true, prints the log message as well as other details * (level, file line, etc). If false, only prints the message */ - ColouredCoutSink(bool print_detailed); + ColouredCoutSink(bool print_detailed, bool reduce_repetition = true); /** * This is a helper function for mapping the FG_Colour enum to its relative * Linux xterm foreground color @@ -63,5 +63,7 @@ class ColouredCoutSink */ bool print_detailed; + bool reduce_repetition; + LogMerger log_merger; }; diff --git a/src/software/logger/coloured_cout_sink_test.cpp b/src/software/logger/coloured_cout_sink_test.cpp index 4d50442277..87d7a14f56 100644 --- a/src/software/logger/coloured_cout_sink_test.cpp +++ b/src/software/logger/coloured_cout_sink_test.cpp @@ -73,8 +73,7 @@ TEST_P(ColouredCoutSinkTest, testLogInfoNoDetails) // LOG(FATAL) isn't tested because it causes a SIGABRT INSTANTIATE_TEST_CASE_P( All, ColouredCoutSinkTest, - ::testing::Values( - std::make_tuple(LEVELS(INFO), FG_Colour::WHITE), - std::make_tuple(LEVELS(DEBUG), FG_Colour::GREEN), - std::make_tuple(LEVELS(ROBOT_STATUS), FG_Colour::WHITE), - std::make_tuple(LEVELS(WARNING), FG_Colour::YELLOW))); + ::testing::Values(std::make_tuple(LEVELS(INFO), FG_Colour::WHITE), + std::make_tuple(LEVELS(DEBUG), FG_Colour::GREEN), + std::make_tuple(LEVELS(WARNING), + FG_Colour::YELLOW))); diff --git a/src/software/logger/csv_sink_test.cpp b/src/software/logger/csv_sink_test.cpp index 98187dbe66..ad9e45693a 100644 --- a/src/software/logger/csv_sink_test.cpp +++ b/src/software/logger/csv_sink_test.cpp @@ -54,5 +54,4 @@ TEST_P(CSVSinkTest, test_csv_log_levels_not_logging) INSTANTIATE_TEST_CASE_P(All, CSVSinkTest, ::testing::Values(std::make_tuple(LEVELS(INFO)), std::make_tuple(LEVELS(DEBUG)), - std::make_tuple(LEVELS(ROBOT_STATUS)), std::make_tuple(LEVELS(WARNING)))); diff --git a/src/software/logger/custom_logging_levels.h b/src/software/logger/custom_logging_levels.h index 0d62273c3c..06fad07935 100644 --- a/src/software/logger/custom_logging_levels.h +++ b/src/software/logger/custom_logging_levels.h @@ -8,7 +8,6 @@ // diagnostic information like battery voltage and wheel encoder errors. These messages // are separate because they are shown separately in the FullSystemGUI. // over radio -const LEVELS ROBOT_STATUS{INFO.value + 1, {"ROBOT_STATUS"}}; -const LEVELS CSV{INFO.value + 2, {"CSV"}}; -const LEVELS VISUALIZE{INFO.value + 3, {"VISUALIZE"}}; -const LEVELS PLOTJUGGLER{INFO.value + 4, {"PLOTJUGGLER"}}; +const LEVELS CSV{INFO.value + 1, {"CSV"}}; +const LEVELS VISUALIZE{INFO.value + 2, {"VISUALIZE"}}; +const LEVELS PLOTJUGGLER{INFO.value + 3, {"PLOTJUGGLER"}}; diff --git a/src/software/logger/logger.h b/src/software/logger/logger.h index 0d121383e3..461ff8f732 100644 --- a/src/software/logger/logger.h +++ b/src/software/logger/logger.h @@ -57,17 +57,22 @@ class LoggerSingleton * called once at the start of a program. * * @param runtime_dir The directory where the log files will be stored. + * @param proto_logger The proto logger to log VISUALIZE protos + * @param reduce_repetition Whether logs should be merged whenever possible to reduce + * spam */ static void initializeLogger(const std::string& runtime_dir, - const std::shared_ptr& proto_logger) + const std::shared_ptr& proto_logger, + const bool reduce_repetition = true) { static std::shared_ptr s( - new LoggerSingleton(runtime_dir, proto_logger)); + new LoggerSingleton(runtime_dir, proto_logger, reduce_repetition)); } private: LoggerSingleton(const std::string& runtime_dir, - const std::shared_ptr& proto_logger) + const std::shared_ptr& proto_logger, + const bool reduce_repetition) { logWorker = g3::LogWorker::createLogWorker(); // Default locations @@ -93,9 +98,9 @@ class LoggerSingleton auto csv_sink_handle = logWorker->addSink(std::make_unique(runtime_dir), &CSVSink::appendToFile); // Sink for outputting logs to the terminal - auto colour_cout_sink_handle = - logWorker->addSink(std::make_unique(true), - &ColouredCoutSink::displayColouredLog); + auto colour_cout_sink_handle = logWorker->addSink( + std::make_unique(true, reduce_repetition), + &ColouredCoutSink::displayColouredLog); // Sink for storing a file of filtered logs auto filtered_log_rotate_sink_handle = logWorker->addSink( @@ -122,10 +127,9 @@ class LoggerSingleton } // levels is this vector are filtered out of the filtered log rotate sink - std::vector filtered_level_filter = {DEBUG, VISUALIZE, CSV, - INFO, ROBOT_STATUS, PLOTJUGGLER}; - std::vector default_level_filter = {VISUALIZE, CSV, ROBOT_STATUS, - PLOTJUGGLER}; + std::vector filtered_level_filter = {DEBUG, VISUALIZE, CSV, INFO, + PLOTJUGGLER}; + std::vector default_level_filter = {VISUALIZE, CSV, PLOTJUGGLER}; const std::string filter_suffix = "_filtered"; const std::string log_name = "thunderbots"; std::unique_ptr logWorker; diff --git a/src/software/multithreading/BUILD b/src/software/multithreading/BUILD index e6b2193ef4..c051206e6c 100644 --- a/src/software/multithreading/BUILD +++ b/src/software/multithreading/BUILD @@ -64,6 +64,7 @@ cc_test( deps = [ ":observer", "//shared/test_util:tbots_gtest_main", + "//software/test_util:fake_clock", ], ) diff --git a/src/software/multithreading/observer.hpp b/src/software/multithreading/observer.hpp index 02cea7784f..8bf17142f2 100644 --- a/src/software/multithreading/observer.hpp +++ b/src/software/multithreading/observer.hpp @@ -8,8 +8,9 @@ * "Subject" to receive new instances of type T when they are available * * @tparam T The type of object this class is observing + * @tparam Clock A clock that satisfies the TrivialClock requirements */ -template +template class Observer { public: @@ -79,34 +80,34 @@ class Observer boost::circular_buffer receive_time_buffer; }; -template -Observer::Observer(size_t buffer_size, bool log_buffer_full) +template +Observer::Observer(size_t buffer_size, bool log_buffer_full) : buffer(buffer_size, log_buffer_full), receive_time_buffer(TIME_BUFFER_SIZE) { } -template -void Observer::receiveValue(T val) +template +void Observer::receiveValue(T val) { receive_time_buffer.push_back(std::chrono::duration_cast( - std::chrono::steady_clock::now().time_since_epoch())); + Clock::now().time_since_epoch())); buffer.push(std::move(val)); } -template -std::optional Observer::popMostRecentlyReceivedValue(Duration max_wait_time) +template +std::optional Observer::popMostRecentlyReceivedValue(Duration max_wait_time) { return buffer.popMostRecentlyAddedValue(max_wait_time); } -template -std::optional Observer::popLeastRecentlyReceivedValue(Duration max_wait_time) +template +std::optional Observer::popLeastRecentlyReceivedValue(Duration max_wait_time) { return buffer.popLeastRecentlyAddedValue(max_wait_time); } -template -double Observer::getDataReceivedPerSecond() +template +double Observer::getDataReceivedPerSecond() { if (receive_time_buffer.empty()) { diff --git a/src/software/multithreading/observer_test.cpp b/src/software/multithreading/observer_test.cpp index f75209c801..277b839689 100644 --- a/src/software/multithreading/observer_test.cpp +++ b/src/software/multithreading/observer_test.cpp @@ -4,7 +4,9 @@ #include -class TestObserver : public Observer +#include "software/test_util/fake_clock.h" + +class TestObserver : public Observer { public: std::optional getMostRecentValueFromBufferWrapper() @@ -19,36 +21,34 @@ namespace TestUtil * Tests getDataReceivedPerSecond by filling the buffer * * @param test_observer The observer to test - * @param data_received_period_milliseconds The period between receiving data + * @param data_received_period_ms The period between receiving data in milliseconds * @param number_of_messages number of messages to send to the buffer * * @return AssertionSuccess the observer returns the correct data received per second */ ::testing::AssertionResult testGetDataReceivedPerSecondByFillingBuffer( - TestObserver test_observer, unsigned int data_received_period_milliseconds, + TestObserver test_observer, unsigned int data_received_period_ms, unsigned int number_of_messages) { - auto wall_time_start = std::chrono::steady_clock::now(); + auto wall_time_start = FakeClock::now(); for (unsigned int i = 0; i < number_of_messages; i++) { test_observer.receiveValue(i); - std::this_thread::sleep_for( - std::chrono::milliseconds(data_received_period_milliseconds)); + FakeClock::advance(std::chrono::milliseconds(data_received_period_ms)); } - auto wall_time_now = std::chrono::steady_clock::now(); + auto wall_time_now = FakeClock::now(); double test_duration_s = static_cast(std::chrono::duration_cast( wall_time_now - wall_time_start) .count()) * SECONDS_PER_MILLISECOND; double scaling_factor = - test_duration_s / (data_received_period_milliseconds * - SECONDS_PER_MILLISECOND * number_of_messages); - double expected_actual_difference = - std::abs(test_observer.getDataReceivedPerSecond() - - 1 / (data_received_period_milliseconds * SECONDS_PER_MILLISECOND) * - scaling_factor); + test_duration_s / + (data_received_period_ms * SECONDS_PER_MILLISECOND * number_of_messages); + double expected_actual_difference = std::abs( + test_observer.getDataReceivedPerSecond() - + 1 / (data_received_period_ms * SECONDS_PER_MILLISECOND) * scaling_factor); if (expected_actual_difference < 50) { return ::testing::AssertionSuccess(); diff --git a/src/software/simulated_tests/requirements_lock.txt b/src/software/simulated_tests/requirements_lock.txt index 4fe396d6b2..8bf6d03071 100644 --- a/src/software/simulated_tests/requirements_lock.txt +++ b/src/software/simulated_tests/requirements_lock.txt @@ -1,5 +1,5 @@ # -# This file is autogenerated by pip-compile with Python 3.10 +# This file is autogenerated by pip-compile with Python 3.12 # by the following command: # # bazel run //software/simulated_tests:requirements.update diff --git a/src/software/simulation/BUILD b/src/software/simulation/BUILD index e10251bfdd..8c2b7a4f92 100644 --- a/src/software/simulation/BUILD +++ b/src/software/simulation/BUILD @@ -9,12 +9,12 @@ cc_library( "//extlibs/er_force_sim/config/simulator:2020B.txt", ], deps = [ - "//extlibs/er_force_sim/src/amun/simulator:simulator_qt", + "//extlibs/er_force_sim/src/amun/simulator", "//proto/message_translation:ssl_detection", "//proto/message_translation:ssl_geometry", "//proto/message_translation:ssl_simulation_robot_control", "//proto/message_translation:ssl_wrapper", - "//software/jetson_nano:primitive_executor", + "//software/embedded:primitive_executor", "//software/physics:euclidean_to_wheel", "//software/physics:velocity_conversion_util", "//software/world", diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index fff9b082a2..eb06a08bff 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -3,8 +3,6 @@ #include #include -#include -#include #include #include "extlibs/er_force_sim/src/protobuf/robot.h" @@ -34,7 +32,7 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, yellow_robot_with_ball(std::nullopt), ramping(ramping) { - QString full_filename = CONFIG_DIRECTORY; + std::string full_filename = CONFIG_DIRECTORY; if (field_type == TbotsProto::FieldType::DIV_A) { @@ -46,19 +44,19 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, full_filename = full_filename + CONFIG_FILE + "B.txt"; } - QFile file(full_filename); - if (!file.open(QFile::ReadOnly)) + std::ifstream config_file(full_filename); + if (!config_file) { - LOG(FATAL) << "Could not open configuration file " << full_filename.toStdString() - << std::endl; + LOG(FATAL) << "Could not open configuration file " << full_filename; } - QString str = file.readAll(); - file.close(); + // Read in the config file + std::stringstream config_ss; + config_ss << config_file.rdbuf(); + std::string config_str = config_ss.str(); - std::string s = qPrintable(str); google::protobuf::TextFormat::Parser parser; - parser.ParseFromString(s, &er_force_sim_setup); + parser.ParseFromString(config_str, &er_force_sim_setup); er_force_sim = std::make_unique(er_force_sim_setup); auto simulator_setup_command = std::make_unique(); simulator_setup_command->mutable_simulator()->set_enable(true); diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 632fc97560..b4e7833012 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -4,7 +4,7 @@ #include "proto/robot_status_msg.pb.h" #include "proto/ssl_vision_wrapper.pb.h" #include "proto/tbots_software_msgs.pb.h" -#include "software/jetson_nano/primitive_executor.h" +#include "software/embedded/primitive_executor.h" #include "software/physics/euclidean_to_wheel.h" #include "software/world/field.h" #include "software/world/team_types.h" @@ -230,6 +230,6 @@ class ErForceSimulator bool ramping; - const QString CONFIG_FILE = "simulator/2020"; - const QString CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; + const std::string CONFIG_FILE = "simulator/2020"; + const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; }; diff --git a/src/software/test_util/BUILD b/src/software/test_util/BUILD index 5cd15e0620..f8d8d1c2ab 100644 --- a/src/software/test_util/BUILD +++ b/src/software/test_util/BUILD @@ -33,6 +33,12 @@ cc_library( ], ) +cc_library( + name = "fake_clock", + srcs = ["fake_clock.cpp"], + hdrs = ["fake_clock.h"], +) + cc_test( name = "test_util_test", srcs = ["test_util_test.cpp"], @@ -50,3 +56,12 @@ cc_test( "//shared/test_util:tbots_gtest_main", ], ) + +cc_test( + name = "fake_clock_test", + srcs = ["fake_clock_test.cpp"], + deps = [ + ":fake_clock", + "//shared/test_util:tbots_gtest_main", + ], +) diff --git a/src/software/test_util/fake_clock.cpp b/src/software/test_util/fake_clock.cpp new file mode 100644 index 0000000000..e3f95068f4 --- /dev/null +++ b/src/software/test_util/fake_clock.cpp @@ -0,0 +1,18 @@ +#include "fake_clock.h" + +FakeClock::time_point FakeClock::now_; + +FakeClock::time_point FakeClock::now() noexcept +{ + return now_; +} + +void FakeClock::advance(duration time) noexcept +{ + now_ += time; +} + +void FakeClock::reset() noexcept +{ + now_ = FakeClock::time_point(); +} diff --git a/src/software/test_util/fake_clock.h b/src/software/test_util/fake_clock.h new file mode 100644 index 0000000000..371cf1fde1 --- /dev/null +++ b/src/software/test_util/fake_clock.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include + +/** + * Fake clock convenient for testing purposes. + * + * Tests that rely on sleeping can be non-deterministic and flaky since they + * depend on real time to elapse. Use this clock instead, which can be explicitly + * advanced without sleeping. + * + * Satisfies the TrivialClock requirements and thus can replace any standard clock + * in the chrono library (e.g. std::chrono::steady_clock). + */ +class FakeClock +{ + public: + using rep = uint64_t; + using period = std::nano; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + inline static const bool is_steady = false; + + /** + * Returns the current time point. + * + * @return the current time point + */ + static time_point now() noexcept; + + /** + * Advances the current time point by the given amount of time. + * + * @param time the amount of time to advance the current time point by + */ + static void advance(duration time) noexcept; + + /** + * Resets the current time point to the clock's epoch + * (the origin of fake_clock::time_point) + */ + static void reset() noexcept; + + private: + FakeClock() = delete; + ~FakeClock() = delete; + FakeClock(FakeClock const&) = delete; + + static time_point now_; +}; diff --git a/src/software/test_util/fake_clock_test.cpp b/src/software/test_util/fake_clock_test.cpp new file mode 100644 index 0000000000..07aa4b16a4 --- /dev/null +++ b/src/software/test_util/fake_clock_test.cpp @@ -0,0 +1,39 @@ +#include "software/test_util/fake_clock.h" + +#include + +TEST(FakeClockTest, time_point_now_should_not_change_without_advancing) +{ + FakeClock::reset(); + FakeClock::time_point t0 = FakeClock::now(); + FakeClock::time_point t1 = FakeClock::now(); + EXPECT_EQ(std::chrono::milliseconds(0), t1 - t0); +} + +TEST(FakeClockTest, time_point_now_should_move_forward_after_advancing) +{ + FakeClock::reset(); + FakeClock::time_point t0 = FakeClock::now(); + + FakeClock::advance(std::chrono::milliseconds(100)); + FakeClock::time_point t1 = FakeClock::now(); + EXPECT_EQ(std::chrono::milliseconds(100), t1 - t0); + + FakeClock::advance(std::chrono::milliseconds(50)); + FakeClock::time_point t2 = FakeClock::now(); + EXPECT_EQ(std::chrono::milliseconds(150), t2 - t0); +} + +TEST(FakeClockTest, reset_should_set_time_point_now_to_clock_epoch) +{ + FakeClock::reset(); + FakeClock::time_point t0 = FakeClock::now(); + + FakeClock::advance(std::chrono::milliseconds(1000)); + FakeClock::time_point t1 = FakeClock::now(); + EXPECT_EQ(std::chrono::milliseconds(1000), t1 - t0); + + FakeClock::reset(); + FakeClock::time_point t2 = FakeClock::now(); + EXPECT_EQ(std::chrono::milliseconds(0), t2 - t0); +} diff --git a/src/software/thunderscope/BUILD b/src/software/thunderscope/BUILD index e188c0d202..1af288edc5 100644 --- a/src/software/thunderscope/BUILD +++ b/src/software/thunderscope/BUILD @@ -142,6 +142,9 @@ py_library( py_library( name = "constants", srcs = ["constants.py"], + deps = [ + requirement("protobuf"), + ], ) py_library( diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index a59463ac9e..5a2f4f3185 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -207,18 +207,18 @@ def send_gc_command( ci_input = CiInput(timestamp=int(time.time_ns())) api_input = Input() change = Change() - new_command = NewCommand() + new_command = Change.NewCommand() command = Command(type=gc_command, for_team=team) new_command.command.CopyFrom(command) - change.new_command.CopyFrom(new_command) + change.new_command_change.CopyFrom(new_command) api_input.change.CopyFrom(change) ci_input.api_inputs.append(api_input) # Do this only if ball placement pos is specified if final_ball_placement_point: # Set position - ball_placement_pos = SetBallPlacementPos() + ball_placement_pos = Change.SetBallPlacementPos() ball_placement_pos.pos.CopyFrom( Vector2( x=float(final_ball_placement_point.x()), @@ -227,16 +227,16 @@ def send_gc_command( ) change = Change() api_input = Input() - change.set_ball_placement_pos.CopyFrom(ball_placement_pos) + change.set_ball_placement_pos_change.CopyFrom(ball_placement_pos) api_input.change.CopyFrom(change) ci_input.api_inputs.append(api_input) # Start Placement - change = Change() api_input = Input() - start_placement = StartBallPlacement() - change.start_ball_placement.CopyFrom(start_placement) - api_input.change.CopyFrom(change) + start_placement = ContinueAction() + start_placement.type = ContinueAction.Type.BALL_PLACEMENT_START + start_placement.for_team = team + api_input.continue_action.CopyFrom(start_placement) ci_input.api_inputs.append(api_input) ci_output_list = self.send_ci_input(ci_input) @@ -264,7 +264,7 @@ def send_ci_input(self, ci_input: proto.ssl_gc_ci_pb2.CiInput) -> list[CiOutput] return ci_output_list - def reset_team(self, name: str, team: str) -> UpdateTeamState: + def reset_team(self, name: str, team: str) -> Change.UpdateTeamState: """Returns an UpdateTeamState proto for the gamecontroller to reset team info. :param name name of the new team @@ -272,27 +272,28 @@ def reset_team(self, name: str, team: str) -> UpdateTeamState: :return: corresponding UpdateTeamState proto """ - update_team_state = UpdateTeamState() + update_team_state = Change.UpdateTeamState() update_team_state.for_team = team - update_team_state.team_name = name - update_team_state.goals = 0 - update_team_state.timeouts_left = 4 - update_team_state.timeout_time_left = "05:00" - update_team_state.can_place_ball = True + update_team_state.team_name.value = name + update_team_state.goals.value = 0 + update_team_state.timeouts_left.value = 4 + update_team_state.timeout_time_left.value = "05:00" + update_team_state.can_place_ball.value = True return update_team_state - def reset_game(self, division: proto.ssl_gc_common_pb2.Division) -> UpdateConfig: + def reset_game( + self, division: proto.ssl_gc_common_pb2.Division + ) -> Change.UpdateConfig: """Returns an UpdateConfig proto for the Gamecontroller to reset game info. :param division the Division proto corresponding to the game division to set up the Gamecontroller for :return: corresponding UpdateConfig proto """ - game_update = UpdateConfig() + game_update = Change.UpdateConfig() game_update.division = division game_update.first_kickoff_team = SslTeam.BLUE - game_update.auto_continue = True game_update.match_type = MatchType.FRIENDLY return game_update @@ -309,19 +310,21 @@ def reset_team_info( ci_input = CiInput(timestamp=int(time.time_ns())) input_blue_update = Input() input_blue_update.reset_match = True - input_blue_update.change.update_team_state.CopyFrom( + input_blue_update.change.update_team_state_change.CopyFrom( self.reset_team("BLUE", SslTeam.BLUE) ) input_yellow_update = Input() input_yellow_update.reset_match = True - input_yellow_update.change.update_team_state.CopyFrom( + input_yellow_update.change.update_team_state_change.CopyFrom( self.reset_team("YELLOW", SslTeam.YELLOW) ) input_game_update = Input() input_game_update.reset_match = True - input_game_update.change.update_config.CopyFrom(self.reset_game(division)) + input_game_update.change.update_config_change.CopyFrom( + self.reset_game(division) + ) ci_input.api_inputs.append(input_blue_update) ci_input.api_inputs.append(input_yellow_update) diff --git a/src/software/thunderscope/binary_context_managers/tigers_autoref.py b/src/software/thunderscope/binary_context_managers/tigers_autoref.py index cff5276956..efaae73314 100644 --- a/src/software/thunderscope/binary_context_managers/tigers_autoref.py +++ b/src/software/thunderscope/binary_context_managers/tigers_autoref.py @@ -101,14 +101,15 @@ def _force_gamecontroller_to_accept_all_events(self) -> list[CiOutput]: :return: a list of CiOutput protos from the Gamecontroller """ - game_event_proto_map = Config() + gc_engine_config = Config() + gc_engine_config.auto_continue = True for game_event in GameEvent.Type.DESCRIPTOR.values_by_name: - game_event_proto_map.game_event_behavior[game_event] = ( + gc_engine_config.game_event_behavior[game_event] = ( Config.Behavior.BEHAVIOR_ACCEPT ) - return self.gamecontroller.update_game_engine_config(game_event_proto_map) + return self.gamecontroller.update_game_engine_config(gc_engine_config) def _send_geometry(self) -> None: """Sends updated field geometry to the AutoRef so that the TigersAutoref knows about field sizes.""" diff --git a/src/software/thunderscope/common/common_widgets.py b/src/software/thunderscope/common/common_widgets.py index 1293301363..21ddcb31dc 100644 --- a/src/software/thunderscope/common/common_widgets.py +++ b/src/software/thunderscope/common/common_widgets.py @@ -145,7 +145,7 @@ def setValue(self, value: float) -> None: :param value: the float value to set """ - super(ColorProgressBar, self).setValue(value * self.decimals) + super(ColorProgressBar, self).setValue(int(value * self.decimals)) # clamp percent to make sure it's between 0% and 100% percent = self.getPercentage() diff --git a/src/software/thunderscope/log/g3log_widget.py b/src/software/thunderscope/log/g3log_widget.py index 62e3bd9941..447d7acb39 100644 --- a/src/software/thunderscope/log/g3log_widget.py +++ b/src/software/thunderscope/log/g3log_widget.py @@ -1,6 +1,5 @@ from pyqtgraph.Qt.QtWidgets import * import queue -import qdarktheme from software.py_constants import * import pyqtgraph.console as pg_console from proto.robot_log_msg_pb2 import RobotLog, LogLevel @@ -20,8 +19,6 @@ def __init__(self, buffer_size: int = 10): """ QWidget.__init__(self) - palette = qdarktheme.load_palette() - self.console_widget = pg_console.ConsoleWidget() self.console_widget.setStyleSheet( """ diff --git a/src/software/thunderscope/play/refereeinfo_widget.py b/src/software/thunderscope/play/refereeinfo_widget.py index 59b56448c7..c2fdf2211b 100644 --- a/src/software/thunderscope/play/refereeinfo_widget.py +++ b/src/software/thunderscope/play/refereeinfo_widget.py @@ -47,9 +47,13 @@ def refresh(self) -> None: if not referee_msg_dict: return + stage_time_left_s = ( + int(referee_msg_dict["stageTimeLeft"]) * SECONDS_PER_MICROSECOND + ) p = ( f"Packet Timestamp: {round(float(referee_msg_dict['packetTimestamp']) * SECONDS_PER_MICROSECOND, 3)}\n" - + f"Stage Time Left: {int(referee_msg_dict['stageTimeLeft'] * SECONDS_PER_MICROSECOND / SECONDS_PER_MINUTE)}:{int(referee_msg_dict['stageTimeLeft'] * SECONDS_PER_MICROSECOND % SECONDS_PER_MINUTE)}\n" + + f"Stage Time Left: {int(stage_time_left_s / SECONDS_PER_MINUTE):02d}" + + f":{int(stage_time_left_s % SECONDS_PER_MINUTE):02d}\n" + f"Stage: {referee_msg_dict['stage']}\n" + "Command: " + referee_msg_dict["command"] diff --git a/src/software/thunderscope/replay/proto_player.py b/src/software/thunderscope/replay/proto_player.py index 1a11354372..5f6c246630 100644 --- a/src/software/thunderscope/replay/proto_player.py +++ b/src/software/thunderscope/replay/proto_player.py @@ -43,8 +43,12 @@ class ProtoPlayer: """ PLAY_PAUSE_POLL_INTERVAL_SECONDS = 0.1 + CHUNK_INDEX_FILENAME = "chunks.index" + CHUNK_INDEX_FILE_VERSION = 1 - def __init__(self, log_folder_path: str, proto_unix_io: ProtoUnixIO) -> None: + def __init__( + self, log_folder_path: os.PathLike, proto_unix_io: ProtoUnixIO + ) -> None: """Creates a proto player that plays back all protos :param log_folder_path: The path to the log file. @@ -76,6 +80,9 @@ def __init__(self, log_folder_path: str, proto_unix_io: ProtoUnixIO) -> None: self.sorted_chunks[0] ) + # build or load index for chunks + self.chunks_indices = self.get_chunk_index() + # We can get the total runtime of the log from the last entry in the last chunk self.end_time = self.find_actual_endtime() @@ -93,14 +100,14 @@ def __init__(self, log_folder_path: str, proto_unix_io: ProtoUnixIO) -> None: self.error_bit_flag = ProtoPlayerFlags.NO_ERROR_FLAG @staticmethod - def sort_and_get_replay_files(log_folder_path): + def sort_and_get_replay_files(log_folder_path: os.PathLike): """Sorting the replay files :param log_folder_path: the path to the folder that we are going to be sorting! :return: the sorted replay files """ # Load up all replay files in the log folder - replay_files = glob.glob(log_folder_path + f"/*.{REPLAY_FILE_EXTENSION}") + replay_files = glob.glob(f"{log_folder_path}/*.{REPLAY_FILE_EXTENSION}") if len(replay_files) == 0: raise ValueError( @@ -109,7 +116,7 @@ def sort_and_get_replay_files(log_folder_path): ) # Sort the files by their chunk index - def __sort_replay_chunks(file_path: str): + def __sort_replay_chunks(file_path: os.PathLike): head, tail = os.path.split(file_path) replay_index, _ = tail.split(".") return int(replay_index) @@ -130,6 +137,99 @@ def is_log_entry_corrupt(log_entry: bytes, version: int) -> bool: except Exception: return True + def build_chunk_index(self, folder_path: os.PathLike) -> dict[str, float]: + """Build the chunk index and store the index into the index file + + Note: + ---- + A chunk index entry is a key value pair [filename: start timestamp] + - Start timestamp: timestamp of the first log entry in the chunk + - Filename: replay chunk file name (%d.replay) + + :param folder_path: folder containing all the replay files + + :return: a dictionary for mapping replay filename to the start + timestamp of the first entry in the chunk. + + """ + chunk_indices: dict[str, float] = dict() + for chunk_name in self.sorted_chunks: + chunk_data = ProtoPlayer.load_replay_chunk(chunk_name, self.version) + if chunk_data: + start_timestamp, _, _ = ProtoPlayer.unpack_log_entry( + chunk_data[0], self.version + ) + chunk_indices[os.path.basename(chunk_name)] = start_timestamp + if chunk_indices: + try: + with open( + os.path.join(folder_path, ProtoPlayer.CHUNK_INDEX_FILENAME), "w" + ) as index_file: + index_file.write( + f"Version: {ProtoPlayer.CHUNK_INDEX_FILE_VERSION}, " + f"Generated on {time.time():.0f}\n" + ) + for filename, start_timestamp in chunk_indices.items(): + index_file.write(f"{start_timestamp}, {filename}\n") + logging.info("Created chunk index file successfully.") + except Exception as e: + logging.warning(f"Failed to build chunk index for {folder_path}: {e}") + else: + logging.warning( + f"Failed to build chunk index for {folder_path} : No chunk data." + ) + return chunk_indices + + def is_chunk_indexed(self) -> bool: + """Returns true if the chunk index is already built. + + :return: if the chunk index file exists + """ + return os.path.exists( + os.path.join(self.log_folder_path, ProtoPlayer.CHUNK_INDEX_FILENAME) + ) + + def load_chunk_index(self) -> dict[str, float]: + """Loads the chunk index file. + + :return: the chunk indices. + """ + chunk_indices: dict[str, float] = dict() + try: + with open( + os.path.join(self.log_folder_path, ProtoPlayer.CHUNK_INDEX_FILENAME), + "r", + ) as index_file: + # skip the first timestamp line + index_file.readline() + + for line in index_file: + start_timestamp, chunk_name = line.split(",") + start_timestamp = float(start_timestamp) + chunk_name = chunk_name.strip() + chunk_indices[chunk_name] = start_timestamp + logging.info("Pre-existing chunk index file found and loaded.") + except Exception as e: + logging.warning(f"An Exception occurred when loading chunk index file {e}") + return chunk_indices + + def get_chunk_index(self) -> dict[str, float]: + """Returns the chunk indices. + NOTE: if the chunk index was not built, this function will automatically build one. + + :return: chunk indices. + """ + if not self.is_chunk_indexed(): + return self.build_chunk_index(self.log_folder_path) + + try: + return self.load_chunk_index() + except Exception as e: + logging.warning( + f"Exception occurred when loading chunk index, trying to rebuild. Message: {e}" + ) + return self.build_chunk_index(self.log_folder_path) + def is_proto_player_playing(self) -> bool: """Return whether or not the proto player is being played. @@ -163,7 +263,7 @@ def find_actual_endtime(self) -> float: return 0.0 @staticmethod - def load_replay_chunk(replay_chunk_path: str, version: int) -> list: + def load_replay_chunk(replay_chunk_path: os.PathLike, version: int) -> list: """Reads a replay chunk. :param replay_chunk_path: The path to the replay chunk. @@ -202,7 +302,7 @@ def load_replay_chunk(replay_chunk_path: str, version: int) -> list: return cached_data @staticmethod - def get_replay_chunk_format_version(replay_chunk_path: str) -> int: + def get_replay_chunk_format_version(replay_chunk_path: os.PathLike) -> int: """Reads a replay chunk. :param replay_chunk_path: The path to the replay chunk. @@ -329,6 +429,7 @@ def save_clip(self, filename: str, start_time: float, end_time: float) -> None: self.current_entry_index += 1 if self.current_packet_time >= end_time: logging.info("Clip saved!") + self.build_chunk_index(directory) return # Load the next chunk self.current_chunk_index += 1 @@ -418,14 +519,24 @@ def seek(self, seek_time: float) -> None: # Let's binary search through the chunks to find the chunk that starts # with a timestamp less than (but closest to) the seek_time we want # to seek to. - def __bisect_chunks_by_timestamp(chunk: str) -> None: - chunk = ProtoPlayer.load_replay_chunk(chunk, self.version) - start_timestamp, _, _ = ProtoPlayer.unpack_log_entry(chunk[0], self.version) - return start_timestamp + def __get_timestamp_for_chunk(chunk: str) -> float: + if self.chunks_indices and os.path.basename(chunk) in self.chunks_indices: + return self.chunks_indices[os.path.basename(chunk)] + else: + logging.warning( + "Use old algorithm to jump, which might result in lagging " + + f"Please try deleting {ProtoPlayer.CHUNK_INDEX_FILENAME} in the replay file folder" + + " and re-run Thunderscope to enable the indexing for faster speed!" + ) + chunk = ProtoPlayer.load_replay_chunk(chunk, self.version) + start_timestamp, _, _ = ProtoPlayer.unpack_log_entry( + chunk[0], self.version + ) + return start_timestamp with self.replay_controls_mutex: self.current_chunk_index = ProtoPlayer.binary_search( - self.sorted_chunks, seek_time, key=__bisect_chunks_by_timestamp + self.sorted_chunks, seek_time, key=__get_timestamp_for_chunk ) # Let's binary search through the entries in the chunk to find the closest diff --git a/src/software/thunderscope/replay/test/BUILD b/src/software/thunderscope/replay/test/BUILD index 3a9408c3ad..159879cd73 100644 --- a/src/software/thunderscope/replay/test/BUILD +++ b/src/software/thunderscope/replay/test/BUILD @@ -19,3 +19,22 @@ py_test( requirement("pytest"), ], ) + +py_test( + name = "replay_indexing_test", + srcs = [ + "replay_indexing_test.py", + ], + data = [ + "//software:py_constants.so", + ], + deps = [ + "//extlibs/er_force_sim/src/protobuf:erforce_py_proto", + "//proto:import_all_protos", + "//software:conftest", + "//software/thunderscope:proto_unix_io", + "//software/thunderscope/replay:proto_player", + "//software/thunderscope/replay/test:replay_corruption_test", + requirement("pytest"), + ], +) diff --git a/src/software/thunderscope/replay/test/replay_corruption_test.py b/src/software/thunderscope/replay/test/replay_corruption_test.py index b54565cf06..da16eb8b2d 100644 --- a/src/software/thunderscope/replay/test/replay_corruption_test.py +++ b/src/software/thunderscope/replay/test/replay_corruption_test.py @@ -9,7 +9,7 @@ 2. we create invalid entries of two type: the data is actually corrupted, and we are missing a delimiter. We mixed those replay entries with valid replay entries to check if proto player could actually player those entries - 4. we check to see if there are uncaught exception. If there are, we know fore sure proto player wouldn't work! + 3. we check to see if there are uncaught exception. If there are, we know for sure proto player wouldn't work! """ import time @@ -94,7 +94,7 @@ def make_part_replay_chunks( save_path: str, duration: float, start_time: float, - gen_log_entry_func: Callable[[Message, float], None], + gen_log_entry_func: Callable[[Message, float], str], frequency=0.1, ): """Making a part of the replay chunks and appending it to the 0.replay file. diff --git a/src/software/thunderscope/replay/test/replay_indexing_test.py b/src/software/thunderscope/replay/test/replay_indexing_test.py new file mode 100644 index 0000000000..04513235c0 --- /dev/null +++ b/src/software/thunderscope/replay/test/replay_indexing_test.py @@ -0,0 +1,126 @@ +"""This test is for testing the chunk indexing function of the proto player. +We are going to examine the index-building and index-loading functions. +Those functions are first introduced to optimize the response time of the progress bar in the replay mode. +""" + +from typing import Callable +import math +import shutil +import gzip +import os +from google.protobuf.message import Message +from software.py_constants import * + +from software.thunderscope.replay.proto_player import ProtoPlayer +from software.thunderscope.proto_unix_io import ProtoUnixIO +from software.simulated_tests.simulated_test_fixture import pytest_main +from software.thunderscope.replay.test.replay_corruption_test import ( + create_valid_log_entry, + create_random_proto, +) + +# location to store the generated files +TMP_REPLAY_SAVE_PATH = "/tmp/test_indexing" + +# number of chunk files being generated +CHUNK_FILES_NUM = 100 + +# number of entries per replay file +ENTRY_NUM_PER_FILE = 100 + +# length of time that every chunk represents +DURATION_PER_CHUNK = 5 + + +def create_test_player() -> ProtoPlayer: + """Create a dummy protoplayer for testing + :return: a protoplayer for testing + """ + return ProtoPlayer(TMP_REPLAY_SAVE_PATH, ProtoUnixIO()) + + +def cleanup(): + """Clean up this test by deleting the replay files generated""" + shutil.rmtree(TMP_REPLAY_SAVE_PATH) + + +def generate_chunk( + list_of_protos: [Message], + save_path: os.PathLike, + filename: str, + duration: float, + start_time: float, + gen_log_entry_func: Callable[[Message, float], str], +): + """Generate one replay chunk for testing. + + :param list_of_protos: the list of proto being referenced when generating log entries + :param save_path: directory for saving the chunk file + :param filename: the name of the chunk file + :param duration: the length of the chunk (in seconds) + :param start_time: the start time (in seconds) + :param gen_log_entry_func: function used to generate log entries + """ + os.makedirs(save_path, exist_ok=True) + path_to_replay_file = os.path.join(save_path, filename) + + with gzip.open(path_to_replay_file, "wb") as log_file: + for i, proto in enumerate(list_of_protos): + proto_time = start_time + duration / len(list_of_protos) * i + log_entry = gen_log_entry_func(proto, proto_time) + log_file.write(bytes(log_entry, encoding="utf-8")) + + +def test_for_building_index_on_valid_chunks(): + """Test building index file on correctly formatted replay files + + Test steps: + 1. generate correctly formatted replay files (chunks) + 2. Build index files + 3. Load index into memory and validate + 3. Test ProtoPlayer.seek() to check if the player can jump correctly to the correct chunk + """ + # generate correctly formatted replay files + replay_proto = [] + for _ in range(ENTRY_NUM_PER_FILE): + replay_proto.append(create_random_proto()) + for i in range(CHUNK_FILES_NUM): + generate_chunk( + replay_proto, + TMP_REPLAY_SAVE_PATH, + f"{i}.replay", + DURATION_PER_CHUNK, + DURATION_PER_CHUNK * i, + create_valid_log_entry, + ) + + player = create_test_player() + + # generate chunk index file + player.build_chunk_index(player.log_folder_path) + + # validate index with load index function + chunk_indices = player.load_chunk_index() + assert len(chunk_indices) == CHUNK_FILES_NUM + assert player.chunks_indices + for filename, start_timestamp in chunk_indices.items(): + index_of_file = int(filename.replace(".replay", "")) + assert math.isclose(start_timestamp, DURATION_PER_CHUNK * index_of_file) + + # test seeking a timestamp + # jump to the middle chunk + player.seek(DURATION_PER_CHUNK * CHUNK_FILES_NUM / 2) + assert player.current_chunk_index == int(CHUNK_FILES_NUM / 2) + + # jump to the start chunk + player.seek(0.0) + assert player.current_chunk_index == 0 + + # jump to the end chunk + player.seek(DURATION_PER_CHUNK * CHUNK_FILES_NUM - 1) + assert player.current_chunk_index == CHUNK_FILES_NUM - 1 + cleanup() + + +if __name__ == "__main__": + pytest_main(__file__) diff --git a/src/software/thunderscope/requirements.in b/src/software/thunderscope/requirements.in index 56d48cb1a0..6b87152786 100644 --- a/src/software/thunderscope/requirements.in +++ b/src/software/thunderscope/requirements.in @@ -1,3 +1,3 @@ -numpy==1.25.0 +numpy==1.26.4 +protobuf==5.27.3 pyqtgraph==0.13.7 -pyqtdarktheme==2.1.0 diff --git a/src/software/thunderscope/requirements_lock.txt b/src/software/thunderscope/requirements_lock.txt index cac8531f7d..90bfb381c3 100644 --- a/src/software/thunderscope/requirements_lock.txt +++ b/src/software/thunderscope/requirements_lock.txt @@ -1,45 +1,61 @@ # -# This file is autogenerated by pip-compile with Python 3.10 +# This file is autogenerated by pip-compile with Python 3.12 # by the following command: # # bazel run //software/thunderscope:requirements.update # -darkdetect==0.7.1 \ - --hash=sha256:3efe69f8ecd5f1b7f4fbb0d1d93f656b0e493c45cc49222380ffe2a529cbc866 \ - --hash=sha256:47be3cf5134432ddb616bbffc927237718407914993c82809983e7ccebf49013 - # via pyqtdarktheme -numpy==1.25.0 \ - --hash=sha256:0ac6edfb35d2a99aaf102b509c8e9319c499ebd4978df4971b94419a116d0790 \ - --hash=sha256:26815c6c8498dc49d81faa76d61078c4f9f0859ce7817919021b9eba72b425e3 \ - --hash=sha256:4aedd08f15d3045a4e9c648f1e04daca2ab1044256959f1f95aafeeb3d794c16 \ - --hash=sha256:4c69fe5f05eea336b7a740e114dec995e2f927003c30702d896892403df6dbf0 \ - --hash=sha256:5177310ac2e63d6603f659fadc1e7bab33dd5a8db4e0596df34214eeab0fee3b \ - --hash=sha256:5aa48bebfb41f93043a796128854b84407d4df730d3fb6e5dc36402f5cd594c0 \ - --hash=sha256:5b1b90860bf7d8a8c313b372d4f27343a54f415b20fb69dd601b7efe1029c91e \ - --hash=sha256:6c284907e37f5e04d2412950960894b143a648dea3f79290757eb878b91acbd1 \ - --hash=sha256:6d183b5c58513f74225c376643234c369468e02947b47942eacbb23c1671f25d \ - --hash=sha256:7412125b4f18aeddca2ecd7219ea2d2708f697943e6f624be41aa5f8a9852cc4 \ - --hash=sha256:7cd981ccc0afe49b9883f14761bb57c964df71124dcd155b0cba2b591f0d64b9 \ - --hash=sha256:85cdae87d8c136fd4da4dad1e48064d700f63e923d5af6c8c782ac0df8044542 \ - --hash=sha256:8aa130c3042052d656751df5e81f6d61edff3e289b5994edcf77f54118a8d9f4 \ - --hash=sha256:95367ccd88c07af21b379be1725b5322362bb83679d36691f124a16357390153 \ - --hash=sha256:9c7211d7920b97aeca7b3773a6783492b5b93baba39e7c36054f6e749fc7490c \ - --hash=sha256:9e3f2b96e3b63c978bc29daaa3700c028fe3f049ea3031b58aa33fe2a5809d24 \ - --hash=sha256:b76aa836a952059d70a2788a2d98cb2a533ccd46222558b6970348939e55fc24 \ - --hash=sha256:b792164e539d99d93e4e5e09ae10f8cbe5466de7d759fc155e075237e0c274e4 \ - --hash=sha256:c0dc071017bc00abb7d7201bac06fa80333c6314477b3d10b52b58fa6a6e38f6 \ - --hash=sha256:cc3fda2b36482891db1060f00f881c77f9423eead4c3579629940a3e12095fe8 \ - --hash=sha256:d6b267f349a99d3908b56645eebf340cb58f01bd1e773b4eea1a905b3f0e4208 \ - --hash=sha256:d76a84998c51b8b68b40448ddd02bd1081bb33abcdc28beee6cd284fe11036c6 \ - --hash=sha256:e559c6afbca484072a98a51b6fa466aae785cfe89b69e8b856c3191bc8872a82 \ - --hash=sha256:ecc68f11404930e9c7ecfc937aa423e1e50158317bf67ca91736a9864eae0232 \ - --hash=sha256:f1accae9a28dc3cda46a91de86acf69de0d1b5f4edd44a9b0c3ceb8036dfff19 +numpy==1.26.4 \ + --hash=sha256:03a8c78d01d9781b28a6989f6fa1bb2c4f2d51201cf99d3dd875df6fbd96b23b \ + --hash=sha256:08beddf13648eb95f8d867350f6a018a4be2e5ad54c8d8caed89ebca558b2818 \ + --hash=sha256:1af303d6b2210eb850fcf03064d364652b7120803a0b872f5211f5234b399f20 \ + --hash=sha256:1dda2e7b4ec9dd512f84935c5f126c8bd8b9f2fc001e9f54af255e8c5f16b0e0 \ + --hash=sha256:2a02aba9ed12e4ac4eb3ea9421c420301a0c6460d9830d74a9df87efa4912010 \ + --hash=sha256:2e4ee3380d6de9c9ec04745830fd9e2eccb3e6cf790d39d7b98ffd19b0dd754a \ + --hash=sha256:3373d5d70a5fe74a2c1bb6d2cfd9609ecf686d47a2d7b1d37a8f3b6bf6003aea \ + --hash=sha256:47711010ad8555514b434df65f7d7b076bb8261df1ca9bb78f53d3b2db02e95c \ + --hash=sha256:4c66707fabe114439db9068ee468c26bbdf909cac0fb58686a42a24de1760c71 \ + --hash=sha256:50193e430acfc1346175fcbdaa28ffec49947a06918b7b92130744e81e640110 \ + --hash=sha256:52b8b60467cd7dd1e9ed082188b4e6bb35aa5cdd01777621a1658910745b90be \ + --hash=sha256:60dedbb91afcbfdc9bc0b1f3f402804070deed7392c23eb7a7f07fa857868e8a \ + --hash=sha256:62b8e4b1e28009ef2846b4c7852046736bab361f7aeadeb6a5b89ebec3c7055a \ + --hash=sha256:666dbfb6ec68962c033a450943ded891bed2d54e6755e35e5835d63f4f6931d5 \ + --hash=sha256:675d61ffbfa78604709862923189bad94014bef562cc35cf61d3a07bba02a7ed \ + --hash=sha256:679b0076f67ecc0138fd2ede3a8fd196dddc2ad3254069bcb9faf9a79b1cebcd \ + --hash=sha256:7349ab0fa0c429c82442a27a9673fc802ffdb7c7775fad780226cb234965e53c \ + --hash=sha256:7ab55401287bfec946ced39700c053796e7cc0e3acbef09993a9ad2adba6ca6e \ + --hash=sha256:7e50d0a0cc3189f9cb0aeb3a6a6af18c16f59f004b866cd2be1c14b36134a4a0 \ + --hash=sha256:95a7476c59002f2f6c590b9b7b998306fba6a5aa646b1e22ddfeaf8f78c3a29c \ + --hash=sha256:96ff0b2ad353d8f990b63294c8986f1ec3cb19d749234014f4e7eb0112ceba5a \ + --hash=sha256:9fad7dcb1aac3c7f0584a5a8133e3a43eeb2fe127f47e3632d43d677c66c102b \ + --hash=sha256:9ff0f4f29c51e2803569d7a51c2304de5554655a60c5d776e35b4a41413830d0 \ + --hash=sha256:a354325ee03388678242a4d7ebcd08b5c727033fcff3b2f536aea978e15ee9e6 \ + --hash=sha256:a4abb4f9001ad2858e7ac189089c42178fcce737e4169dc61321660f1a96c7d2 \ + --hash=sha256:ab47dbe5cc8210f55aa58e4805fe224dac469cde56b9f731a4c098b91917159a \ + --hash=sha256:afedb719a9dcfc7eaf2287b839d8198e06dcd4cb5d276a3df279231138e83d30 \ + --hash=sha256:b3ce300f3644fb06443ee2222c2201dd3a89ea6040541412b8fa189341847218 \ + --hash=sha256:b97fe8060236edf3662adfc2c633f56a08ae30560c56310562cb4f95500022d5 \ + --hash=sha256:bfe25acf8b437eb2a8b2d49d443800a5f18508cd811fea3181723922a8a82b07 \ + --hash=sha256:cd25bcecc4974d09257ffcd1f098ee778f7834c3ad767fe5db785be9a4aa9cb2 \ + --hash=sha256:d209d8969599b27ad20994c8e41936ee0964e6da07478d6c35016bc386b66ad4 \ + --hash=sha256:d5241e0a80d808d70546c697135da2c613f30e28251ff8307eb72ba696945764 \ + --hash=sha256:edd8b5fe47dab091176d21bb6de568acdd906d1887a4584a15a9a96a1dca06ef \ + --hash=sha256:f870204a840a60da0b12273ef34f7051e98c3b5961b61b0c2c1be6dfd64fbcd3 \ + --hash=sha256:ffa75af20b44f8dba823498024771d5ac50620e6915abac414251bd971b4529f # via # -r software/thunderscope/requirements.in # pyqtgraph -pyqtdarktheme==2.1.0 \ - --hash=sha256:5f8274ddfa3a5481ed9743cdb0f9debfeb7ff695b3a0d202a8104361d17dadb8 \ - --hash=sha256:8739d99502230fbaca42551ea033c9ae31c81c4ebfec2f1ffde38f32a18bea7a +protobuf==5.27.3 \ + --hash=sha256:043853dcb55cc262bf2e116215ad43fa0859caab79bb0b2d31b708f128ece035 \ + --hash=sha256:16ddf3f8c6c41e1e803da7abea17b1793a97ef079a912e42351eabb19b2cffe7 \ + --hash=sha256:68248c60d53f6168f565a8c76dc58ba4fa2ade31c2d1ebdae6d80f969cdc2d4f \ + --hash=sha256:82460903e640f2b7e34ee81a947fdaad89de796d324bcbc38ff5430bcdead82c \ + --hash=sha256:8572c6533e544ebf6899c360e91d6bcbbee2549251643d32c52cf8a5de295ba5 \ + --hash=sha256:a55c48f2a2092d8e213bd143474df33a6ae751b781dd1d1f4d953c128a415b25 \ + --hash=sha256:af7c0b7cfbbb649ad26132e53faa348580f844d9ca46fd3ec7ca48a1ea5db8a1 \ + --hash=sha256:b8a994fb3d1c11156e7d1e427186662b64694a62b55936b2b9348f0a7c6625ce \ + --hash=sha256:c2a105c24f08b1e53d6c7ffe69cb09d0031512f0b72f812dd4005b8112dbe91e \ + --hash=sha256:c84eee2c71ed83704f1afbf1a85c3171eab0fd1ade3b399b3fad0884cbcca8bf \ + --hash=sha256:dcb307cd4ef8fec0cf52cb9105a03d06fbb5275ce6d84a6ae33bc6cf84e0a07b # via -r software/thunderscope/requirements.in pyqtgraph==0.13.7 \ --hash=sha256:64f84f1935c6996d0e09b1ee66fe478a7771e3ca6f3aaa05f00f6e068321d9e3 \ diff --git a/src/software/thunderscope/robot_diagnostics/chicker_widget.py b/src/software/thunderscope/robot_diagnostics/chicker_widget.py index fddaa582af..8d22cc102f 100644 --- a/src/software/thunderscope/robot_diagnostics/chicker_widget.py +++ b/src/software/thunderscope/robot_diagnostics/chicker_widget.py @@ -122,7 +122,7 @@ def send_command_and_timeout(self, command: ChickerCommandMode) -> None: self.disable_kick_chip_buttons() # set and start timer to re-enable buttons after 3 seconds - QTimer.singleShot(CHICKER_TIMEOUT, self.enable_kick_chip_buttons) + QTimer.singleShot(int(CHICKER_TIMEOUT), self.enable_kick_chip_buttons) def disable_kick_chip_buttons(self) -> None: """Disables the buttons""" diff --git a/src/software/thunderscope/robot_diagnostics/robot_info.py b/src/software/thunderscope/robot_diagnostics/robot_info.py index efa620dbc1..d139871fd5 100644 --- a/src/software/thunderscope/robot_diagnostics/robot_info.py +++ b/src/software/thunderscope/robot_diagnostics/robot_info.py @@ -307,7 +307,7 @@ def update(self, robot_status: RobotStatus, round_trip_time: RobotStatistic): self.__update_ui(robot_status, round_trip_time) - QtCore.QTimer.singleShot(DISCONNECT_DURATION_MS, self.disconnect_robot) + QtCore.QTimer.singleShot(int(DISCONNECT_DURATION_MS), self.disconnect_robot) def disconnect_robot(self) -> None: """Calculates the time between the last robot status and now diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index 89783501a8..f7d893b5ef 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -11,9 +11,9 @@ WidgetStretchData, ) import pyqtgraph +from qt_material import apply_stylesheet import signal import os -import qdarktheme @dataclass @@ -39,7 +39,11 @@ def initialize_application() -> None: app = pyqtgraph.mkQApp("Thunderscope") # Setup stylesheet - qdarktheme.setup_theme() + extra = { + # Make thunderscope more dense + "density_scale": "-2", + } + apply_stylesheet(app, theme="dark_blue.xml", extra=extra) def configure_robot_view_fullsystem( @@ -475,6 +479,7 @@ def configure_replay_view( initialize_application() if blue_replay_log: + blue_refresh_func_counter = FrameTimeCounter() proto_unix_io_map[ProtoUnixIOTypes.BLUE] = ProtoUnixIO() tabs.append( TScopeTab( @@ -487,11 +492,15 @@ def configure_replay_view( replay_log=blue_replay_log, visualization_buffer_size=visualization_buffer_size, extra_widgets=[], + frame_swap_counter=FrameTimeCounter(), + refresh_counter=blue_refresh_func_counter, ), + refresh_counter=blue_refresh_func_counter, ) ) if yellow_replay_log: + yellow_refresh_func_counter = FrameTimeCounter() proto_unix_io_map[ProtoUnixIOTypes.YELLOW] = ProtoUnixIO() tabs.append( TScopeTab( @@ -506,7 +515,10 @@ def configure_replay_view( replay_log=yellow_replay_log, visualization_buffer_size=visualization_buffer_size, extra_widgets=[], + frame_swap_counter=FrameTimeCounter(), + refresh_counter=yellow_refresh_func_counter, ), + refresh_counter=yellow_refresh_func_counter, ) ) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 78b70a4e32..8645d6ce8d 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -4,6 +4,17 @@ import os import sys import threading + +import google.protobuf +from google.protobuf.internal import api_implementation + +protobuf_impl_type = api_implementation.Type() +assert protobuf_impl_type == "upb", ( + f"Trying to use the {protobuf_impl_type} protobuf implementation. " + "Please use the upb implementation, available in python protobuf version 4.21.0 and above." + f"The current version of protobuf is {google.protobuf.__version__}" +) + from software.thunderscope.thunderscope import Thunderscope from software.thunderscope.binary_context_managers import * from proto.import_all_protos import * diff --git a/src/software/util/make_enum/make_enum.hpp b/src/software/util/make_enum/make_enum.hpp index 7cba24dda9..39cb6eb431 100644 --- a/src/software/util/make_enum/make_enum.hpp +++ b/src/software/util/make_enum/make_enum.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include "software/util/make_enum/make_enum_helpers.hpp" /** diff --git a/src/software/util/make_enum/make_enum_helpers.hpp b/src/software/util/make_enum/make_enum_helpers.hpp index a80ca85f2e..ad0c679993 100644 --- a/src/software/util/make_enum/make_enum_helpers.hpp +++ b/src/software/util/make_enum/make_enum_helpers.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include diff --git a/src/tbots.py b/src/tbots.py index 9bdeb13710..874ce21ff9 100755 --- a/src/tbots.py +++ b/src/tbots.py @@ -56,7 +56,7 @@ nargs="+", type=int, help="A list of space separated integers representing the robot IDs " - "that should be flashed by the deploy_nano Ansible playbook", + "that should be flashed by the deploy_robot_software Ansible playbook", action="store", ) parser.add_argument( @@ -77,6 +77,14 @@ help="Run the binary with the TRACY_ENABLE macro defined", action="store_true", ) + parser.add_argument( + "-pl", + "--platform", + type=str, + choices=["PI", "NANO"], + help="The platform to build Thunderloop for", + action="store", + ) # These are shortcut args for commonly used arguments on our tests # and full_system. All other arguments are passed through as-is @@ -159,7 +167,7 @@ # Used for when flashing Jetsons if args.flash_robots: - command += ["--cpu=jetson_nano"] + command += ["--platforms=//cc_toolchain:robot"] # Select debug binaries to run if args.select_debug_binaries: @@ -174,6 +182,9 @@ if args.tracy: command += ["--cxxopt=-DTRACY_ENABLE"] + if args.platform: + command += ["--//software/embedded:host_platform=" + args.platform] + # Don't cache test results if args.action in "test": command += ["--cache_test_results=false"] @@ -181,7 +192,6 @@ command += ["--"] bazel_arguments = unknown_args - if args.stop_ai_on_start: bazel_arguments += ["--stop_ai_on_start"] if args.enable_visualizer: @@ -189,9 +199,13 @@ if args.enable_thunderscope: bazel_arguments += ["--enable_thunderscope"] if args.flash_robots: - bazel_arguments += ["-pb deploy_nano.yml"] + if not args.platform: + print("No platform specified! Make sure to set the --platform argument.") + sys.exit(1) + bazel_arguments += ["-pb deploy_robot_software.yml"] bazel_arguments += ["--hosts"] - bazel_arguments += [f"192.168.0.20{id}" for id in args.flash_robots] + platform_ip = "0" if args.platform == "NANO" else "5" + bazel_arguments += [f"192.168.{platform_ip}.20{id}" for id in args.flash_robots] bazel_arguments += ["-pwd", args.pwd] if args.action in "test":