From c5c3e9ff49f56bc1a01a51ab7ff50f08b997d0c0 Mon Sep 17 00:00:00 2001 From: JulianTrommer Date: Mon, 14 Oct 2024 17:06:46 +0200 Subject: [PATCH 1/5] Added black action & refactored actions --- .github/workflows/add-to-project.yml | 2 +- .github/workflows/build.yml | 90 ++-------------------------- .github/workflows/drive.yaml | 63 +++++++++++++++++++ .github/workflows/format.yaml | 21 +++++++ .github/workflows/linter.yml | 18 +++--- 5 files changed, 100 insertions(+), 94 deletions(-) create mode 100644 .github/workflows/drive.yaml create mode 100644 .github/workflows/format.yaml diff --git a/.github/workflows/add-to-project.yml b/.github/workflows/add-to-project.yml index dcced1c4..52f9bcf6 100644 --- a/.github/workflows/add-to-project.yml +++ b/.github/workflows/add-to-project.yml @@ -1,4 +1,4 @@ -name: Add bugs to bugs project +name: Add issue to project on: issues: diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index fa07e68f..ac3b7276 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -1,9 +1,10 @@ -name: Build, publish and run tests +name: Build and push image on: - push: - branches: [ 'main' ] - pull_request: + workflow_run: + workflows: ["Check code format", "Linter markdown and code"] + types: + - completed env: REGISTRY: ghcr.io @@ -38,19 +39,6 @@ jobs: username: ${{ github.actor }} password: ${{ secrets.GITHUB_TOKEN }} - - name: Bump version and push tag - # only run on push to main - if: github.event_name == 'push' && github.ref == 'refs/heads/main' - id: tag - uses: mathieudutour/github-tag-action@v6.1 - with: - github_token: ${{ secrets.GITHUB_TOKEN }} - release_branches: main - - - name: Get commit hash - id: hash - run: echo "hash=$(git rev-parse --short HEAD)" >> $GITHUB_OUTPUT - - name: Build and push Docker image id: build uses: docker/build-push-action@v3 @@ -59,72 +47,6 @@ jobs: file: ./build/docker/build/Dockerfile push: true # tag 'latest' and version on push to main, otherwise use the commit hash - tags: | - ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}:${{ steps.tag.outputs.new_version == '' && steps.hash.outputs.hash || steps.tag.outputs.new_version }} - ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}:${{ github.event_name == 'push' && github.ref == 'refs/heads/main' && 'latest' || steps.hash.outputs.hash }} + tags: ${{ env.REGISTRY }}/${{ env.IMAGE_NAME }}:latest cache-from: type=gha cache-to: type=gha,mode=max - - - name: Output version - id: version - # take either the created tag or the commit hash - run: echo "version=${{ steps.tag.outputs.new_version == '' && steps.hash.outputs.hash || steps.tag.outputs.new_version }}" >> $GITHUB_OUTPUT - drive: - runs-on: self-hosted - needs: build-and-push-image - # run only on pull request for now - if: github.event_name == 'pull_request' - env: - AGENT_VERSION: ${{ needs.build-and-push-image.outputs.version }} - COMPOSE_FILE: ./build/docker-compose.cicd.yaml - steps: - - name: Checkout repository - uses: actions/checkout@v3 - - name: Print environment variables (DEBUG) - run: | - echo "AGENT_VERSION=${AGENT_VERSION}" - echo "COMPOSE_FILE=${COMPOSE_FILE}" - - name: Get commit hash - id: hash - run: echo "hash=$(git rev-parse --short HEAD)" >> $GITHUB_OUTPUT - - name: Set AGENT_VERSION from hash (workaround) - run: echo "AGENT_VERSION=${{ steps.hash.outputs.hash }}" >> $GITHUB_ENV - - name: Run docker-compose - run: docker compose up --quiet-pull --exit-code-from agent - - name: Copy results - run: docker compose cp agent:/tmp/simulation_results.json . - - name: Stop docker-compose - # always run this step, to clean up even on error - if: always() - run: docker compose down -v - # add rendered JSON as comment to the pull request - - name: Add simulation results as comment - if: github.event_name == 'pull_request' - uses: actions/github-script@v6 - with: - github-token: ${{ secrets.GITHUB_TOKEN }} - # this script reads the simulation_results.json and creates a comment on the pull request with the results. - script: | - const fs = require('fs'); - // read the simulation results - const results = fs.readFileSync('./simulation_results.json', 'utf8'); - let resultsJson = JSON.parse(results); - // create a markdown table of the results - let resultsTable = resultsJson.values.map((values, i) => { - return `| ${resultsJson.labels[i]} | ${values} |`; - }); - // create a markdown table header - let resultsTableHeader = `| Metric | Value |`; - // create a markdown table divider - let resultsTableDivider = `| --- | --- |`; - // add everything to the resultsTable - resultsTable = resultsTableHeader + '\n' + resultsTableDivider + '\n' + resultsTable.join('\n'); - // add the results as a comment to the pull request - github.rest.issues.createComment({ - issue_number: context.issue.number, - owner: context.repo.owner, - repo: context.repo.repo, - body: "## Simulation results\n" + resultsTable - }); - - name: Prune all images older than 30 days from self-hosted runner - run: docker image prune -a --force --filter "until=720h" diff --git a/.github/workflows/drive.yaml b/.github/workflows/drive.yaml new file mode 100644 index 00000000..629acba7 --- /dev/null +++ b/.github/workflows/drive.yaml @@ -0,0 +1,63 @@ +name: Evaluate agent + +on: + workflow_run: + workflows: ["Build and push image"] + types: + - completed + +jobs: + drive: + runs-on: self-hosted + needs: build-and-push-image + # run only on pull request for now + if: github.event_name == 'pull_request' + env: + AGENT_VERSION: latest + COMPOSE_FILE: ./build/docker-compose.cicd.yaml + steps: + - name: Checkout repository + uses: actions/checkout@v3 + - name: Print environment variables (DEBUG) + run: | + echo "AGENT_VERSION=${AGENT_VERSION}" + echo "COMPOSE_FILE=${COMPOSE_FILE}" + - name: Run docker-compose + run: docker compose up --quiet-pull --exit-code-from agent + - name: Copy results + run: docker compose cp agent:/tmp/simulation_results.json . + - name: Stop docker-compose + # always run this step, to clean up even on error + if: always() + run: docker compose down -v + # add rendered JSON as comment to the pull request + - name: Add simulation results as comment + if: github.event_name == 'pull_request' + uses: actions/github-script@v6 + with: + github-token: ${{ secrets.GITHUB_TOKEN }} + # this script reads the simulation_results.json and creates a comment on the pull request with the results. + script: | + const fs = require('fs'); + // read the simulation results + const results = fs.readFileSync('./simulation_results.json', 'utf8'); + let resultsJson = JSON.parse(results); + // create a markdown table of the results + let resultsTable = resultsJson.values.map((values, i) => { + return `| ${resultsJson.labels[i]} | ${values} |`; + }); + // create a markdown table header + let resultsTableHeader = `| Metric | Value |`; + // create a markdown table divider + let resultsTableDivider = `| --- | --- |`; + // add everything to the resultsTable + resultsTable = resultsTableHeader + '\n' + resultsTableDivider + '\n' + resultsTable.join('\n'); + // add the results as a comment to the pull request + github.rest.issues.createComment({ + issue_number: context.issue.number, + owner: context.repo.owner, + repo: context.repo.repo, + body: "## Simulation results\n" + resultsTable + }); + - name: Prune all images older than 30 days from self-hosted runner + run: docker image prune -a --force --filter "until=720h" \ No newline at end of file diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml new file mode 100644 index 00000000..58a1947e --- /dev/null +++ b/.github/workflows/format.yaml @@ -0,0 +1,21 @@ +name: Check code format + +on: + pull_request: + branches: + - "main" + +jobs: + format: + name: Check code files format + runs-on: ubuntu-latest + steps: + - name: Check out the repo + uses: actions/checkout@v2 + # Execute the python formatter + - name: Run the python formatter + uses: addnab/docker-run-action@v3 + with: + image: pyfound/black + options: -v ${{ github.workspace}}:/apps + run: black --check ./apps/ diff --git a/.github/workflows/linter.yml b/.github/workflows/linter.yml index d20a1b54..630ad725 100644 --- a/.github/workflows/linter.yml +++ b/.github/workflows/linter.yml @@ -1,6 +1,9 @@ -name: linter +name: Linter markdown and code -on: pull_request +on: + pull_request: + branches: + - "main" jobs: linter: @@ -13,16 +16,13 @@ jobs: - name: Run the markdown linter uses: addnab/docker-run-action@v3 with: - image: peterdavehello/markdownlint:0.32.2 - options: -v ${{ github.workspace }}:/md - run: | - markdownlint . + image: peterdavehello/markdownlint:0.32.2 + options: -v ${{ github.workspace }}:/md + run: markdownlint . # Execute the python linter (executes even if the previous step failed) - name: Run the python linter - if: always() uses: addnab/docker-run-action@v3 with: image: alpine/flake8 options: -v ${{ github.workspace }}:/apps - run: | - flake8 code + run: flake8 . From 60501bb685d596c1732080edcf19f29040c75fa5 Mon Sep 17 00:00:00 2001 From: JulianTrommer Date: Mon, 14 Oct 2024 17:07:31 +0200 Subject: [PATCH 2/5] Fixed issues with file permissions & black linter --- .vscode/settings.json | 2 +- build/agent_service.yaml | 3 --- build/docker-compose.dev.yaml | 8 ++++---- build/docker-compose.devroute-distributed.yaml | 2 +- build/docker-compose.devroute.yaml | 2 +- build/docker-compose.leaderboard-distributed.yaml | 2 +- build/docker-compose.leaderboard.yaml | 2 +- build/docker-compose.linter.yaml | 6 ++++++ build/docker/agent/Dockerfile | 6 +++--- 9 files changed, 18 insertions(+), 15 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 8d42024f..60b7e5d1 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -23,7 +23,7 @@ "docker.commands.composeUp": [ { "label": "Compose Up", - "template": "xhost +local: && ${composeCommand} ${configurationFile} up" + "template": "xhost +local: && USERNAME=$(whoami) USER_UID=$(id -u) USER_GID=$(id -g) ${composeCommand} ${configurationFile} up" } ], "workbench.iconTheme": "vscode-icons" diff --git a/build/agent_service.yaml b/build/agent_service.yaml index 6362983b..8266fe9d 100644 --- a/build/agent_service.yaml +++ b/build/agent_service.yaml @@ -2,9 +2,6 @@ services: agent: build: dockerfile: build/docker/agent/Dockerfile - args: - - USER_UID=${DOCKER_HOST_UNIX_UID:-1000} - - USER_GID=${DOCKER_HOST_UNIX_GID:-1000} context: ../ init: true tty: true diff --git a/build/docker-compose.dev.yaml b/build/docker-compose.dev.yaml index 0e3fe39a..95d06f11 100644 --- a/build/docker-compose.dev.yaml +++ b/build/docker-compose.dev.yaml @@ -6,8 +6,9 @@ services: dockerfile: build/docker/agent-dev/Dockerfile context: ../ args: - - USER_UID=${DOCKER_HOST_UNIX_UID:-1000} - - USER_GID=${DOCKER_HOST_UNIX_GID:-1000} + USERNAME: ${USERNAME} + USER_UID: ${USER_UID} + USER_GID: ${USER_GID} init: true tty: true shm_size: 2gb @@ -24,5 +25,4 @@ services: network_mode: host privileged: true entrypoint: ["/dev_entrypoint.sh"] - command: bash - \ No newline at end of file + command: bash -c "sudo chown -R ${USER_UID}:${USER_GID} ../ && sudo chmod -R a+w ../ && bash" diff --git a/build/docker-compose.devroute-distributed.yaml b/build/docker-compose.devroute-distributed.yaml index e8ed7e9f..cc9f429c 100644 --- a/build/docker-compose.devroute-distributed.yaml +++ b/build/docker-compose.devroute-distributed.yaml @@ -8,7 +8,7 @@ services: extends: file: agent_service.yaml service: agent - command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + command: bash -c "sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && sudo chmod -R a+w ../ && roslaunch agent/launch/dev.launch" environment: - CARLA_SIM_HOST= - ROUTE=/workspace/code/routes/routes_simple.xml diff --git a/build/docker-compose.devroute.yaml b/build/docker-compose.devroute.yaml index 6f04d601..4510f0a2 100644 --- a/build/docker-compose.devroute.yaml +++ b/build/docker-compose.devroute.yaml @@ -12,4 +12,4 @@ services: service: agent environment: - ROUTE=/workspace/code/routes/routes_simple.xml - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP" + command: bash -c "sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && sudo chmod -R a+w ../ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP" diff --git a/build/docker-compose.leaderboard-distributed.yaml b/build/docker-compose.leaderboard-distributed.yaml index 464c1aa6..1bcb9949 100644 --- a/build/docker-compose.leaderboard-distributed.yaml +++ b/build/docker-compose.leaderboard-distributed.yaml @@ -7,7 +7,7 @@ services: extends: file: agent_service.yaml service: agent - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP" + command: bash -c "sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && sudo chmod -R a+w ../ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP" environment: - CARLA_SIM_HOST= diff --git a/build/docker-compose.leaderboard.yaml b/build/docker-compose.leaderboard.yaml index 93a669fc..32fc98fc 100644 --- a/build/docker-compose.leaderboard.yaml +++ b/build/docker-compose.leaderboard.yaml @@ -8,4 +8,4 @@ services: extends: file: agent_service.yaml service: agent - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP" + command: bash -c "sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && sudo chmod -R a+w ../ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP" diff --git a/build/docker-compose.linter.yaml b/build/docker-compose.linter.yaml index 0816aaab..d184141d 100644 --- a/build/docker-compose.linter.yaml +++ b/build/docker-compose.linter.yaml @@ -5,6 +5,12 @@ services: volumes: - ../:/apps + black: + image: pyfound/black + command: black --check ./apps/ + volumes: + - ../:/apps + mdlint: image: peterdavehello/markdownlint:0.32.2 command: markdownlint . diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 1917b372..7f7d60d7 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -12,9 +12,9 @@ FROM osrf/ros:noetic-desktop-full-focal # COPY --from=carla /home/carla/PythonAPI /opt/carla/PythonAPI -ARG USERNAME=carla -ARG USER_UID=999 -ARG USER_GID=$USER_UID +ARG USERNAME +ARG USER_UID +ARG USER_GID ARG DEBIAN_FRONTEND=noninteractive # install rendering dependencies for rviz / rqt From e97dae5a61f1bc6fb9f59d12cafd7ff9300733b9 Mon Sep 17 00:00:00 2001 From: JulianTrommer Date: Tue, 15 Oct 2024 08:25:40 +0200 Subject: [PATCH 3/5] Added black formatter to project --- .editorconfig | 2 +- .flake8 | 8 ++----- .vscode/extensions.json | 3 ++- .vscode/settings.json | 3 ++- build/docker/agent/Dockerfile | 42 +++++++++++++++++------------------ 5 files changed, 28 insertions(+), 30 deletions(-) diff --git a/.editorconfig b/.editorconfig index 9dd330ea..8db9e725 100644 --- a/.editorconfig +++ b/.editorconfig @@ -10,7 +10,7 @@ insert_final_newline = true trim_trailing_whitespace = true [*.py] -max_line_length = 80 +max_line_length = 88 indent_size = 4 [*.md] diff --git a/.flake8 b/.flake8 index 042f2345..6c032f36 100644 --- a/.flake8 +++ b/.flake8 @@ -1,7 +1,3 @@ [flake8] -exclude= code/planning/src/behavior_agent/behavior_tree.py, - code/planning/src/behavior_agent/behaviours/__init__.py, - code/planning/src/behavior_agent/behaviours, - code/planning/__init__.py, - doc/development/templates/template_class_no_comments.py, - doc/development/templates/template_class.py \ No newline at end of file +max-line-length = 88 +extend-ignore = E203,E701 \ No newline at end of file diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 6e4fc554..c27bbad5 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -11,6 +11,7 @@ "bierner.markdown-mermaid", "richardkotze.git-mob", "ms-vscode-remote.remote-containers", - "valentjn.vscode-ltex" + "valentjn.vscode-ltex", + "ms-python.black-formatter" ] } \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 60b7e5d1..817fbe31 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -26,5 +26,6 @@ "template": "xhost +local: && USERNAME=$(whoami) USER_UID=$(id -u) USER_GID=$(id -g) ${composeCommand} ${configurationFile} up" } ], - "workbench.iconTheme": "vscode-icons" + "workbench.iconTheme": "vscode-icons", + "editor.formatOnSave": true } \ No newline at end of file diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 7f7d60d7..c8c05ceb 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -19,7 +19,7 @@ ARG DEBIAN_FRONTEND=noninteractive # install rendering dependencies for rviz / rqt RUN apt-get update \ - && apt-get install -y -qq --no-install-recommends \ + && apt-get install -y -qq --no-install-recommends \ libxext6 libx11-6 libglvnd0 libgl1 \ libglx0 libegl1 freeglut3-dev apt-utils \ fprintd libfprint-2-2 libpam-fprintd @@ -30,10 +30,10 @@ RUN apt-get install wget unzip # Download Carla PythonAPI (alternative to getting it from the Carla-Image, which is commented out above) # If the PythonAPI/Carla version changes, either update the link, or refer to the comment at the top of this file. RUN wget https://github.com/una-auxme/paf/releases/download/v0.0.1/PythonAPI_Leaderboard-2.0.zip -O PythonAPI.zip \ - && unzip PythonAPI.zip \ - && rm PythonAPI.zip \ - && mkdir -p /opt/carla \ - && mv PythonAPI /opt/carla/PythonAPI + && unzip PythonAPI.zip \ + && rm PythonAPI.zip \ + && mkdir -p /opt/carla \ + && mv PythonAPI /opt/carla/PythonAPI # Workaround/fix for using dpkg for cuda installation # Only required for the lab PCs @@ -65,12 +65,12 @@ ENV PYTHONPATH=$PYTHONPATH:/opt/carla/PythonAPI/carla/dist/carla-0.9.14-py3.7-li # install mlocate, pip, wget, git and some ROS dependencies for building the CARLA ROS bridge RUN apt-get update && apt-get install -y \ - mlocate python3-pip wget git python-is-python3 \ - ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ - ros-noetic-carla-msgs ros-noetic-pcl-conversions \ - ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ - ros-noetic-robot-pose-ekf ros-noetic-ros-numpy \ - ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure + mlocate python3-pip wget git python-is-python3 \ + ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ + ros-noetic-carla-msgs ros-noetic-pcl-conversions \ + ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ + ros-noetic-robot-pose-ekf ros-noetic-ros-numpy \ + ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure SHELL ["/bin/bash", "-c"] @@ -105,9 +105,9 @@ ENV CARLA_ROS_BRIDGE_ROOT=/catkin_ws/src/ros-bridge # (as we're not running as root, pip installs into ~/.local/bin) ENV PATH=$PATH:/home/$USERNAME/.local/bin -# install simple_pid +# install pip packages RUN python -m pip install pip --upgrade \ - && python -m pip install simple_pid pygame transformations roslibpy lxml + && python -m pip install simple_pid pygame transformations roslibpy lxml black # install the scenario runner from GitHub leaderboard-2.0 branch ENV CARLA_ROOT=/opt/carla @@ -179,11 +179,11 @@ RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc ENTRYPOINT ["/entrypoint.sh"] CMD ["bash", "-c", "sleep 10 && \ -python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py \ ---debug=${DEBUG_CHALLENGE} \ ---repetitions=${REPETITIONS} \ ---checkpoint=${CHECKPOINT_ENDPOINT} \ ---track=${CHALLENGE_TRACK} \ ---agent=${TEAM_AGENT} \ ---routes=${ROUTES} \ ---host=${CARLA_SIM_HOST}"] + python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py \ + --debug=${DEBUG_CHALLENGE} \ + --repetitions=${REPETITIONS} \ + --checkpoint=${CHECKPOINT_ENDPOINT} \ + --track=${CHALLENGE_TRACK} \ + --agent=${TEAM_AGENT} \ + --routes=${ROUTES} \ + --host=${CARLA_SIM_HOST}"] From 5d522ebbd3919365f57c3a4fbb94325bb8827b3d Mon Sep 17 00:00:00 2001 From: JulianTrommer Date: Tue, 15 Oct 2024 06:31:01 +0000 Subject: [PATCH 4/5] Applied black formatting --- code/acting/setup.py | 3 +- code/acting/src/acting/Acting_Debug_Node.py | 167 +++-- code/acting/src/acting/MainFramePublisher.py | 54 +- code/acting/src/acting/helper_functions.py | 61 +- .../src/acting/pure_pursuit_controller.py | 96 +-- code/acting/src/acting/stanley_controller.py | 103 +-- code/acting/src/acting/vehicle_controller.py | 98 +-- code/acting/src/acting/velocity_controller.py | 51 +- code/agent/setup.py | 3 +- code/agent/src/agent/agent.py | 146 ++-- code/mock/setup.py | 3 +- code/mock/src/mock_intersection_clear.py | 20 +- code/mock/src/mock_stop_sign.py | 21 +- code/mock/src/mock_traffic_light.py | 21 +- code/perception/setup.py | 3 +- .../src/coordinate_transformation.py | 13 +- code/perception/src/dataset_converter.py | 57 +- code/perception/src/dataset_generator.py | 168 +++-- .../Position_Heading_Datasets/viz.py | 396 ++++++----- .../src/global_plan_distance_publisher.py | 47 +- code/perception/src/kalman_filter.py | 118 ++-- code/perception/src/lidar_distance.py | 157 +++-- code/perception/src/lidar_filter_utility.py | 21 +- .../src/position_heading_filter_debug_node.py | 272 ++++---- .../src/position_heading_publisher_node.py | 79 ++- .../src/data_generation/weights_organizer.py | 22 +- .../src/traffic_light_config.py | 2 +- .../classification_model.py | 26 +- .../traffic_light_inference.py | 57 +- .../traffic_light_training.py | 107 +-- .../src/traffic_light_detection/transforms.py | 4 +- code/perception/src/traffic_light_node.py | 25 +- code/perception/src/vision_node.py | 242 +++---- code/planning/setup.py | 3 +- .../src/behavior_agent/behavior_tree.py | 165 +++-- .../behaviours/behavior_speed.py | 1 - .../behavior_agent/behaviours/intersection.py | 133 ++-- .../behavior_agent/behaviours/lane_change.py | 69 +- .../behavior_agent/behaviours/maneuvers.py | 157 +++-- .../src/behavior_agent/behaviours/meta.py | 26 +- .../src/behavior_agent/behaviours/overtake.py | 70 +- .../behaviours/road_features.py | 66 +- .../behaviours/topics2blackboard.py | 124 ++-- .../behaviours/traffic_objects.py | 35 +- .../src/global_planner/dev_global_route.py | 79 ++- .../src/global_planner/global_planner.py | 136 ++-- .../src/global_planner/help_functions.py | 86 +-- .../global_planner/preplanning_trajectory.py | 636 ++++++++++-------- code/planning/src/local_planner/ACC.py | 77 ++- .../src/local_planner/collision_check.py | 84 +-- .../src/local_planner/motion_planning.py | 159 +++-- code/planning/src/local_planner/utils.py | 52 +- code/test-route/src/test_route.py | 82 ++- doc/development/templates/template_class.py | 1 + .../templates/template_class_no_comments.py | 8 +- .../globals.py | 18 +- .../object-detection-model_evaluation/pt.py | 56 +- .../pylot.py | 70 +- .../object-detection-model_evaluation/yolo.py | 39 +- doc/research/paf23/planning/test_traj.py | 58 +- 60 files changed, 2825 insertions(+), 2328 deletions(-) diff --git a/code/acting/setup.py b/code/acting/setup.py index e2665b1f..773f1357 100644 --- a/code/acting/setup.py +++ b/code/acting/setup.py @@ -2,6 +2,5 @@ from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup(packages=['acting'], - package_dir={'': 'src'}) +setup_args = generate_distutils_setup(packages=["acting"], package_dir={"": "src"}) setup(**setup_args) diff --git a/code/acting/src/acting/Acting_Debug_Node.py b/code/acting/src/acting/Acting_Debug_Node.py index 99839e18..b3289747 100755 --- a/code/acting/src/acting/Acting_Debug_Node.py +++ b/code/acting/src/acting/Acting_Debug_Node.py @@ -71,35 +71,33 @@ def __init__(self): Constructor of the class :return: """ - super(Acting_Debug_Node, self).__init__('dummy_trajectory_pub') - self.loginfo('Acting_Debug_Node node started') - self.role_name = self.get_param('role_name', 'ego_vehicle') - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) + super(Acting_Debug_Node, self).__init__("dummy_trajectory_pub") + self.loginfo("Acting_Debug_Node node started") + self.role_name = self.get_param("role_name", "ego_vehicle") + self.control_loop_rate = self.get_param("control_loop_rate", 0.05) # Publisher for Dummy Trajectory self.trajectory_pub: Publisher = self.new_publisher( - Path, - "/paf/" + self.role_name + "/trajectory", - qos_profile=1) + Path, "/paf/" + self.role_name + "/trajectory", qos_profile=1 + ) # Publisher for Dummy Velocity self.velocity_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/target_velocity", - qos_profile=1) + Float32, f"/paf/{self.role_name}/target_velocity", qos_profile=1 + ) # PurePursuit: Publisher for Dummy PP-Steer self.pure_pursuit_steer_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/pure_pursuit_steer", - qos_profile=1) + Float32, f"/paf/{self.role_name}/pure_pursuit_steer", qos_profile=1 + ) # Subscriber of current_pos, used for Steering Debugging self.current_pos_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, - qos_profile=1) + qos_profile=1, + ) # ---> EVALUATION/TUNING: Subscribers for plotting # Subscriber for target_velocity for plotting @@ -107,55 +105,61 @@ def __init__(self): Float32, f"/paf/{self.role_name}/target_velocity", self.__get_target_velocity, - qos_profile=1) + qos_profile=1, + ) # Subscriber for current_heading self.heading_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/current_heading", self.__get_heading, - qos_profile=1) + qos_profile=1, + ) # Subscriber for current_velocity self.current_velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__get_current_velocity, - qos_profile=1) + qos_profile=1, + ) # Subscriber for current_throttle self.current_throttle_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/throttle", self.__get_throttle, - qos_profile=1) + qos_profile=1, + ) # Subscriber for Stanley_steer self.stanley_steer_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/stanley_steer", self.__get_stanley_steer, - qos_profile=1) + qos_profile=1, + ) # Subscriber for PurePursuit_steer self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/pure_pursuit_steer", self.__get_purepursuit_steer, - qos_profile=1) + qos_profile=1, + ) # Subscriber for vehicle_steer self.vehicle_steer_sub: Subscriber = self.new_subscription( CarlaEgoVehicleControl, - f'/carla/{self.role_name}/vehicle_control_cmd', + f"/carla/{self.role_name}/vehicle_control_cmd", self.__get_vehicle_steer, - qos_profile=10) + qos_profile=10, + ) # Publisher for emergency brake testing self.emergency_pub: Publisher = self.new_publisher( - Bool, - f"/paf/{self.role_name}/emergency", - qos_profile=1) + Bool, f"/paf/{self.role_name}/emergency", qos_profile=1 + ) # Initialize all needed "global" variables here self.current_trajectory = [] @@ -181,16 +185,12 @@ def __init__(self): # Spawncoords at the simulationstart startx = 984.5 starty = -5442.0 - if (TRAJECTORY_TYPE == 0): # Straight trajectory - self.current_trajectory = [ - (startx, starty), - (startx, starty-200) - ] + if TRAJECTORY_TYPE == 0: # Straight trajectory + self.current_trajectory = [(startx, starty), (startx, starty - 200)] - elif (TRAJECTORY_TYPE == 1): # straight into 90° Curve + elif TRAJECTORY_TYPE == 1: # straight into 90° Curve self.current_trajectory = [ (984.5, -5442.0), - (984.5, -5563.5), (985.0, -5573.2), (986.3, -5576.5), @@ -198,12 +198,11 @@ def __init__(self): (988.7, -5579.0), (990.5, -5579.8), (1000.0, -5580.2), - (1040.0, -5580.0), - (1070.0, -5580.0) + (1070.0, -5580.0), ] - elif (TRAJECTORY_TYPE == 2): # Sinewave Serpentines trajectory + elif TRAJECTORY_TYPE == 2: # Sinewave Serpentines trajectory # Generate a sine-wave with the global Constants to # automatically generate a trajectory with serpentine waves cycles = 4 # how many sine cycles @@ -224,53 +223,50 @@ def __init__(self): traj_y -= 2 trajectory_wave.append((traj_x, traj_y)) # back to the middle of the road - trajectory_wave.append((startx, traj_y-2)) + trajectory_wave.append((startx, traj_y - 2)) # add a long straight path after the serpentines - trajectory_wave.append((startx, starty-200)) + trajectory_wave.append((startx, starty - 200)) self.current_trajectory = trajectory_wave - elif (TRAJECTORY_TYPE == 3): # 2 Lane Switches + elif TRAJECTORY_TYPE == 3: # 2 Lane Switches self.current_trajectory = [ (startx, starty), - (startx-0.5, starty-10), - (startx-0.5, starty-20), - - (startx-0.4, starty-21), - (startx-0.3, starty-22), - (startx-0.2, starty-23), - (startx-0.1, starty-24), - (startx, starty-25), - (startx+0.1, starty-26), - (startx+0.2, starty-27), - (startx+0.3, starty-28), - (startx+0.4, starty-29), - (startx+0.5, starty-30), - (startx+0.6, starty-31), - (startx+0.7, starty-32), - (startx+0.8, starty-33), - (startx+0.9, starty-34), - (startx+1.0, starty-35), - (startx+1.0, starty-50), - - (startx+1.0, starty-51), - (startx+0.9, starty-52), - (startx+0.8, starty-53), - (startx+0.7, starty-54), - (startx+0.6, starty-55), - (startx+0.5, starty-56), - (startx+0.4, starty-57), - (startx+0.3, starty-58), - (startx+0.2, starty-59), - (startx+0.1, starty-60), - (startx, starty-61), - (startx-0.1, starty-62), - (startx-0.2, starty-63), - (startx-0.3, starty-64), - (startx-0.4, starty-65), - (startx-0.5, starty-66), - - (startx-0.5, starty-100), - ] + (startx - 0.5, starty - 10), + (startx - 0.5, starty - 20), + (startx - 0.4, starty - 21), + (startx - 0.3, starty - 22), + (startx - 0.2, starty - 23), + (startx - 0.1, starty - 24), + (startx, starty - 25), + (startx + 0.1, starty - 26), + (startx + 0.2, starty - 27), + (startx + 0.3, starty - 28), + (startx + 0.4, starty - 29), + (startx + 0.5, starty - 30), + (startx + 0.6, starty - 31), + (startx + 0.7, starty - 32), + (startx + 0.8, starty - 33), + (startx + 0.9, starty - 34), + (startx + 1.0, starty - 35), + (startx + 1.0, starty - 50), + (startx + 1.0, starty - 51), + (startx + 0.9, starty - 52), + (startx + 0.8, starty - 53), + (startx + 0.7, starty - 54), + (startx + 0.6, starty - 55), + (startx + 0.5, starty - 56), + (startx + 0.4, starty - 57), + (startx + 0.3, starty - 58), + (startx + 0.2, starty - 59), + (startx + 0.1, starty - 60), + (startx, starty - 61), + (startx - 0.1, starty - 62), + (startx - 0.2, starty - 63), + (startx - 0.3, starty - 64), + (startx - 0.4, starty - 65), + (startx - 0.5, starty - 66), + (startx - 0.5, starty - 100), + ] self.updated_trajectory(self.current_trajectory) def updated_trajectory(self, target_trajectory): @@ -347,21 +343,21 @@ def loop(timer_event=None): depending on the selected TEST_TYPE """ # Drive const. velocity on fixed straight steering - if (TEST_TYPE == 0): + if TEST_TYPE == 0: self.driveVel = TARGET_VELOCITY_1 self.pure_pursuit_steer_pub.publish(FIXED_STEERING) self.velocity_pub.publish(self.driveVel) # Drive alternating velocities on fixed straight steering - elif (TEST_TYPE == 1): + elif TEST_TYPE == 1: if not self.time_set: self.drive_Vel = TARGET_VELOCITY_1 self.switch_checkpoint_time = rospy.get_time() self.switch_time_set = True - if (self.switch_checkpoint_time < rospy.get_time() - 10): + if self.switch_checkpoint_time < rospy.get_time() - 10: self.switch_checkpoint_time = rospy.get_time() self.switchVelocity = not self.switchVelocity - if (self.switchVelocity): + if self.switchVelocity: self.driveVel = TARGET_VELOCITY_2 else: self.driveVel = TARGET_VELOCITY_1 @@ -369,7 +365,7 @@ def loop(timer_event=None): self.velocity_pub.publish(self.driveVel) # drive const. velocity on trajectoy with steering controller - elif (TEST_TYPE == 2): + elif TEST_TYPE == 2: # Continuously update path and publish it self.drive_Vel = TARGET_VELOCITY_1 self.updated_trajectory(self.current_trajectory) @@ -378,13 +374,13 @@ def loop(timer_event=None): # drive const. velocity on fixed straight steering and # trigger an emergency brake after 15 secs - elif (TEST_TYPE == 3): + elif TEST_TYPE == 3: # Continuously update path and publish it self.drive_Vel = TARGET_VELOCITY_1 if not self.time_set: self.checkpoint_time = rospy.get_time() self.time_set = True - if (self.checkpoint_time < rospy.get_time() - 15.0): + if self.checkpoint_time < rospy.get_time() - 15.0: self.checkpoint_time = rospy.get_time() self.emergency_pub.publish(True) self.pure_pursuit_steer_pub.publish(FIXED_STEERING) @@ -402,7 +398,7 @@ def loop(timer_event=None): print(">>>>>>>>>>>> TRAJECTORY <<<<<<<<<<<<<<") # Uncomment the prints of the data you want to plot - if (self.checkpoint_time < rospy.get_time() - PRINT_AFTER_TIME): + if self.checkpoint_time < rospy.get_time() - PRINT_AFTER_TIME: self.checkpoint_time = rospy.get_time() print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") if PRINT_VELOCITY_DATA: @@ -420,6 +416,7 @@ def loop(timer_event=None): print(">> ACTUAL POSITIONS <<") print(self.positions) print(">>>>>>>>>>>> DATA <<<<<<<<<<<<<<") + self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/acting/src/acting/MainFramePublisher.py b/code/acting/src/acting/MainFramePublisher.py index 0ba4783d..a34240e8 100755 --- a/code/acting/src/acting/MainFramePublisher.py +++ b/code/acting/src/acting/MainFramePublisher.py @@ -19,11 +19,11 @@ def __init__(self): ego vehicle does. The hero frame is used by sensors like the lidar. Rviz also uses the hero frame. The main frame is used for planning. """ - super(MainFramePublisher, self).__init__('main_frame_publisher') - self.loginfo('MainFramePublisher node started') + super(MainFramePublisher, self).__init__("main_frame_publisher") + self.loginfo("MainFramePublisher node started") - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) - self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param("control_loop_rate", 0.05) + self.role_name = self.get_param("role_name", "ego_vehicle") self.current_pos: PoseStamped = PoseStamped() self.current_heading: float = 0 @@ -31,16 +31,18 @@ def __init__(self): PoseStamped, "/paf/" + self.role_name + "/current_pos", self.get_current_pos, - qos_profile=1) + qos_profile=1, + ) self.current_heading_subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/current_heading", self.get_current_heading, - qos_profile=1) + qos_profile=1, + ) def run(self): - self.loginfo('MainFramePublisher node running') + self.loginfo("MainFramePublisher node running") br = tf.TransformBroadcaster() def loop(timer_event=None): @@ -49,22 +51,26 @@ def loop(timer_event=None): return rot = -self.current_heading pos = [0, 0, 0] - pos[0] = cos(rot) * \ - self.current_pos.pose.position.x - \ - sin(rot) * self.current_pos.pose.position.y - pos[1] = sin(rot) * \ - self.current_pos.pose.position.x + \ - cos(rot) * self.current_pos.pose.position.y + pos[0] = ( + cos(rot) * self.current_pos.pose.position.x + - sin(rot) * self.current_pos.pose.position.y + ) + pos[1] = ( + sin(rot) * self.current_pos.pose.position.x + + cos(rot) * self.current_pos.pose.position.y + ) pos[2] = -self.current_pos.pose.position.z - rot_quat = R.from_euler("xyz", [0, 0, -self.current_heading+pi], - degrees=False).as_quat() - - br.sendTransform(pos, - rot_quat, - rospy.Time.now(), - "global", - "hero", - ) + rot_quat = R.from_euler( + "xyz", [0, 0, -self.current_heading + pi], degrees=False + ).as_quat() + + br.sendTransform( + pos, + rot_quat, + rospy.Time.now(), + "global", + "hero", + ) self.new_timer(self.control_loop_rate, loop) self.spin() @@ -81,7 +87,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('main_frame_publisher', args=args) + roscomp.init("main_frame_publisher", args=args) try: node = MainFramePublisher() @@ -92,5 +98,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/acting/src/acting/helper_functions.py b/code/acting/src/acting/helper_functions.py index 072e9aa2..8dc2c6d8 100755 --- a/code/acting/src/acting/helper_functions.py +++ b/code/acting/src/acting/helper_functions.py @@ -106,10 +106,10 @@ def calc_path_yaw(path: Path, idx: int) -> float: point_current = path.poses[idx] point_next: PoseStamped point_next = path.poses[idx + 1] - angle = math.atan2(point_next.pose.position.y - - point_current.pose.position.y, - point_next.pose.position.x - - point_current.pose.position.x) + angle = math.atan2( + point_next.pose.position.y - point_current.pose.position.y, + point_next.pose.position.x - point_current.pose.position.x, + ) return normalize_angle(angle) @@ -134,14 +134,19 @@ def calc_egocar_yaw(pose: PoseStamped) -> float: :param pose: The current pose of the ego vehicle :return: normalized yaw of the vehicle """ - quaternion = (pose.pose.orientation.x, pose.pose.orientation.y, - pose.pose.orientation.z, pose.pose.orientation.w) + quaternion = ( + pose.pose.orientation.x, + pose.pose.orientation.y, + pose.pose.orientation.z, + pose.pose.orientation.w, + ) _, _, yaw = euler_from_quaternion(quaternion) return normalize_angle(yaw) -def points_to_vector(p_1: Tuple[float, float], - p_2: Tuple[float, float]) -> Tuple[float, float]: +def points_to_vector( + p_1: Tuple[float, float], p_2: Tuple[float, float] +) -> Tuple[float, float]: """ Create the vector starting at p1 and ending at p2 :param p_1: Start point @@ -157,11 +162,12 @@ def vector_len(vec: Tuple[float, float]) -> float: :param vec: vector v as a tuple (x, y) :return: length of vector v """ - return sqrt(vec[0]**2 + vec[1]**2) + return sqrt(vec[0] ** 2 + vec[1] ** 2) -def add_vector(v_1: Tuple[float, float], - v_2: Tuple[float, float]) -> Tuple[float, float]: +def add_vector( + v_1: Tuple[float, float], v_2: Tuple[float, float] +) -> Tuple[float, float]: """ Add the two given vectors :param v_1: first vector @@ -172,20 +178,22 @@ def add_vector(v_1: Tuple[float, float], return v_1[0] + v_2[0], v_1[1] + v_2[1] -def rotate_vector(vector: Tuple[float, float], - angle_rad: float) -> Tuple[float, float]: +def rotate_vector(vector: Tuple[float, float], angle_rad: float) -> Tuple[float, float]: """ Rotate the given vector by an angle :param vector: vector :param angle_rad: angle of rotation :return: rotated angle """ - return (cos(angle_rad) * vector[0] - sin(angle_rad) * vector[1], - sin(angle_rad) * vector[0] + cos(angle_rad) * vector[1]) + return ( + cos(angle_rad) * vector[0] - sin(angle_rad) * vector[1], + sin(angle_rad) * vector[0] + cos(angle_rad) * vector[1], + ) -def linear_interpolation(start: Tuple[float, float], end: Tuple[float, float], - interval_m: float) -> List[Tuple[float, float]]: +def linear_interpolation( + start: Tuple[float, float], end: Tuple[float, float], interval_m: float +) -> List[Tuple[float, float]]: """ Interpolate linearly between start and end, with a minimal distance of interval_m between points. @@ -200,21 +208,23 @@ def linear_interpolation(start: Tuple[float, float], end: Tuple[float, float], steps = max(1, floor(distance / interval_m)) exceeds_interval_cap = distance > interval_m - step_vector = (vector[0] / steps if exceeds_interval_cap else vector[0], - vector[1] / steps if exceeds_interval_cap else vector[1]) + step_vector = ( + vector[0] / steps if exceeds_interval_cap else vector[0], + vector[1] / steps if exceeds_interval_cap else vector[1], + ) lin_points = [(start[0], start[1])] for i in range(1, steps): lin_points.append( - (start[0] + step_vector[0] * i, - start[1] + step_vector[1] * i) + (start[0] + step_vector[0] * i, start[1] + step_vector[1] * i) ) return lin_points -def _clean_route_duplicates(route: List[Tuple[float, float]], - min_dist: float) -> List[Tuple[float, float]]: +def _clean_route_duplicates( + route: List[Tuple[float, float]], min_dist: float +) -> List[Tuple[float, float]]: """ Remove duplicates in the given List of tuples, if the distance between them is less than min_dist. @@ -243,8 +253,9 @@ def interpolate_route(orig_route: List[Tuple[float, float]], interval_m=0.5): orig_route = _clean_route_duplicates(orig_route, 0.1) route = [] for index in range(len(orig_route) - 1): - waypoints = linear_interpolation(orig_route[index], - orig_route[index + 1], interval_m) + waypoints = linear_interpolation( + orig_route[index], orig_route[index + 1], interval_m + ) route.extend(waypoints) route = route + [orig_route[-1]] diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index 04740418..4259fe78 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -26,45 +26,44 @@ class PurePursuitController(CompatibleNode): def __init__(self): - super(PurePursuitController, self).__init__('pure_pursuit_controller') - self.loginfo('PurePursuitController node started') + super(PurePursuitController, self).__init__("pure_pursuit_controller") + self.loginfo("PurePursuitController node started") - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) - self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param("control_loop_rate", 0.05) + self.role_name = self.get_param("role_name", "ego_vehicle") self.position_sub: Subscriber = self.new_subscription( - Path, - f"/paf/{self.role_name}/trajectory", - self.__set_path, - qos_profile=1) + Path, f"/paf/{self.role_name}/trajectory", self.__set_path, qos_profile=1 + ) self.path_sub: Subscriber = self.new_subscription( PoseStamped, f"/paf/{self.role_name}/current_pos", self.__set_position, - qos_profile=1) + qos_profile=1, + ) self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__set_velocity, - qos_profile=1) + qos_profile=1, + ) self.heading_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/current_heading", self.__set_heading, - qos_profile=1) + qos_profile=1, + ) self.pure_pursuit_steer_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/pure_pursuit_steer", - qos_profile=1) + Float32, f"/paf/{self.role_name}/pure_pursuit_steer", qos_profile=1 + ) self.debug_msg_pub: Publisher = self.new_publisher( - Debug, - f"/paf/{self.role_name}/pure_p_debug", - qos_profile=1) + Debug, f"/paf/{self.role_name}/pure_p_debug", qos_profile=1 + ) self.__position: tuple[float, float] = None # x, y self.__path: Path = None @@ -77,7 +76,7 @@ def run(self): Starts the main loop of the node :return: """ - self.loginfo('PurePursuitController node running') + self.loginfo("PurePursuitController node running") def loop(timer_event=None): """ @@ -86,26 +85,34 @@ def loop(timer_event=None): :return: """ if self.__path is None: - self.logdebug("PurePursuitController hasn't received a path " - "yet and can therefore not publish steering") + self.logdebug( + "PurePursuitController hasn't received a path " + "yet and can therefore not publish steering" + ) return if self.__position is None: - self.logdebug("PurePursuitController hasn't received the " - "position of the vehicle yet " - "and can therefore not publish steering") + self.logdebug( + "PurePursuitController hasn't received the " + "position of the vehicle yet " + "and can therefore not publish steering" + ) return if self.__heading is None: - self.logdebug("PurePursuitController hasn't received the " - "heading of the vehicle yet and " - "can therefore not publish steering") + self.logdebug( + "PurePursuitController hasn't received the " + "heading of the vehicle yet and " + "can therefore not publish steering" + ) return if self.__velocity is None: - self.logdebug("PurePursuitController hasn't received the " - "velocity of the vehicle yet " - "and can therefore not publish steering") + self.logdebug( + "PurePursuitController hasn't received the " + "velocity of the vehicle yet " + "and can therefore not publish steering" + ) return self.pure_pursuit_steer_pub.publish(self.__calculate_steer()) @@ -119,16 +126,17 @@ def __calculate_steer(self) -> float: :return: """ # la_dist = MIN_LA_DISTANCE <= K_LAD * velocity <= MAX_LA_DISTANCE - look_ahead_dist = np.clip(K_LAD * self.__velocity, - MIN_LA_DISTANCE, MAX_LA_DISTANCE) + look_ahead_dist = np.clip( + K_LAD * self.__velocity, MIN_LA_DISTANCE, MAX_LA_DISTANCE + ) # Get the target position on the trajectory in look_ahead distance self.__tp_idx = self.__get_target_point_index(look_ahead_dist) target_wp: PoseStamped = self.__path.poses[self.__tp_idx] # Get the vector from the current position to the target position - target_v_x, target_v_y = points_to_vector((self.__position[0], - self.__position[1]), - (target_wp.pose.position.x, - target_wp.pose.position.y)) + target_v_x, target_v_y = points_to_vector( + (self.__position[0], self.__position[1]), + (target_wp.pose.position.x, target_wp.pose.position.y), + ) # Get the target heading from that vector target_vector_heading = vector_angle(target_v_x, target_v_y) # Get the error between current heading and target heading @@ -181,7 +189,7 @@ def __dist_to(self, pos: Point) -> float: y_current = self.__position[1] x_target = pos.x y_target = pos.y - d = (x_target - x_current)**2 + (y_target - y_current)**2 + d = (x_target - x_current) ** 2 + (y_target - y_current) ** 2 return math.sqrt(d) def __set_position(self, data: PoseStamped, min_diff=0.001): @@ -206,9 +214,11 @@ def __set_position(self, data: PoseStamped, min_diff=0.001): # if new position is to close to current, do not accept it # too close = closer than min_diff = 0.001 meters # for debugging purposes: - self.logdebug("New position disregarded, " - f"as dist ({round(dist, 3)}) to current pos " - f"< min_diff ({round(min_diff, 3)})") + self.logdebug( + "New position disregarded, " + f"as dist ({round(dist, 3)}) to current pos " + f"< min_diff ({round(min_diff, 3)})" + ) return new_x = data.pose.position.x new_y = data.pose.position.y @@ -230,10 +240,10 @@ def __set_velocity(self, data: CarlaSpeedometer): def main(args=None): """ - main function starts the pure pursuit controller node - :param args: + main function starts the pure pursuit controller node + :param args: """ - roscomp.init('pure_pursuit_controller', args=args) + roscomp.init("pure_pursuit_controller", args=args) try: node = PurePursuitController() @@ -244,5 +254,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index e0cbc190..3463e90e 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -20,46 +20,45 @@ class StanleyController(CompatibleNode): def __init__(self): - super(StanleyController, self).__init__('stanley_controller') - self.loginfo('StanleyController node started') + super(StanleyController, self).__init__("stanley_controller") + self.loginfo("StanleyController node started") - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) - self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param("control_loop_rate", 0.05) + self.role_name = self.get_param("role_name", "ego_vehicle") # Subscribers self.position_sub: Subscriber = self.new_subscription( - Path, - f"/paf/{self.role_name}/trajectory", - self.__set_path, - qos_profile=1) + Path, f"/paf/{self.role_name}/trajectory", self.__set_path, qos_profile=1 + ) self.path_sub: Subscriber = self.new_subscription( PoseStamped, f"/paf/{self.role_name}/current_pos", self.__set_position, - qos_profile=1) + qos_profile=1, + ) self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__set_velocity, - qos_profile=1) + qos_profile=1, + ) self.heading_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/current_heading", self.__set_heading, - qos_profile=1) + qos_profile=1, + ) self.stanley_steer_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/stanley_steer", - qos_profile=1) + Float32, f"/paf/{self.role_name}/stanley_steer", qos_profile=1 + ) self.debug_publisher: Publisher = self.new_publisher( - StanleyDebug, - f"/paf/{self.role_name}/stanley_debug", - qos_profile=1) + StanleyDebug, f"/paf/{self.role_name}/stanley_debug", qos_profile=1 + ) self.__position: tuple[float, float] = None # x , y self.__path: Path = None @@ -71,7 +70,7 @@ def run(self): Starts the main loop of the node :return: """ - self.loginfo('StanleyController node running') + self.loginfo("StanleyController node running") def loop(timer_event=None): """ @@ -80,25 +79,33 @@ def loop(timer_event=None): :return: """ if self.__path is None: - self.logwarn("StanleyController hasn't received a path yet " - "and can therefore not publish steering") + self.logwarn( + "StanleyController hasn't received a path yet " + "and can therefore not publish steering" + ) return if self.__position is None: - self.logwarn("StanleyController hasn't received the" - "position of the vehicle yet " - "and can therefore not publish steering") + self.logwarn( + "StanleyController hasn't received the" + "position of the vehicle yet " + "and can therefore not publish steering" + ) return if self.__heading is None: - self.logwarn("StanleyController hasn't received the" - "heading of the vehicle yet and" - "can therefore not publish steering") + self.logwarn( + "StanleyController hasn't received the" + "heading of the vehicle yet and" + "can therefore not publish steering" + ) return if self.__velocity is None: - self.logwarn("StanleyController hasn't received the " - "velocity of the vehicle yet " - "and can therefore not publish steering") + self.logwarn( + "StanleyController hasn't received the " + "velocity of the vehicle yet " + "and can therefore not publish steering" + ) return self.stanley_steer_pub.publish(self.__calculate_steer()) @@ -125,8 +132,9 @@ def __calculate_steer(self) -> float: closest_point: PoseStamped = self.__path.poses[closest_point_idx] cross_err = self.__get_cross_err(closest_point.pose.position) # * -1 because it is inverted compared to PurePursuit - steering_angle = 1 * (heading_err + atan((K_CROSSERR * cross_err) - / current_velocity)) + steering_angle = 1 * ( + heading_err + atan((K_CROSSERR * cross_err) / current_velocity) + ) # -> for debugging debug_msg = StanleyDebug() debug_msg.heading = self.__heading @@ -174,10 +182,11 @@ def __get_path_heading(self, index: int) -> float: if index > 0: # Calculate heading from the previous point on the trajectory - prv_point: Point = self.__path.poses[index-1].pose.position + prv_point: Point = self.__path.poses[index - 1].pose.position - prv_v_x, prv_v_y = points_to_vector((prv_point.x, prv_point.y), - (cur_pos.x, cur_pos.y)) + prv_v_x, prv_v_y = points_to_vector( + (prv_point.x, prv_point.y), (cur_pos.x, cur_pos.y) + ) heading_sum += vector_angle(prv_v_x, prv_v_y) heading_sum_args += 1 @@ -186,8 +195,9 @@ def __get_path_heading(self, index: int) -> float: # Calculate heading to the following point on the trajectory aft_point: Point = self.__path.poses[index + 1].pose.position - aft_v_x, aft_v_y = points_to_vector((aft_point.x, aft_point.y), - (cur_pos.x, cur_pos.y)) + aft_v_x, aft_v_y = points_to_vector( + (aft_point.x, aft_point.y), (cur_pos.x, cur_pos.y) + ) heading_sum += vector_angle(aft_v_x, aft_v_y) heading_sum_args += 1 @@ -210,8 +220,10 @@ def __get_cross_err(self, pos: Point) -> float: if self.__heading is not None: alpha = self.__heading + (math.pi / 2) v_e_0 = (0, 1) - v_e = (cos(alpha)*v_e_0[0] - sin(alpha)*v_e_0[1], - sin(alpha)*v_e_0[0] + cos(alpha)*v_e_0[1]) + v_e = ( + cos(alpha) * v_e_0[0] - sin(alpha) * v_e_0[1], + sin(alpha) * v_e_0[0] + cos(alpha) * v_e_0[1], + ) # define a vector (v_ab) with length 10 centered on the cur pos # of the vehicle, with a heading parallel to that of the vehicle @@ -221,8 +233,7 @@ def __get_cross_err(self, pos: Point) -> float: v_ab = (b[0] - a[0], b[1] - a[1]) v_am = (pos.x - a[0], pos.y - a[1]) - c = np.array([[v_ab[0], v_am[0]], - [v_ab[1], v_am[1]]]) + c = np.array([[v_ab[0], v_am[0]], [v_ab[1], v_am[1]]]) temp_sign = np.linalg.det(c) min_sign = 0.01 # to avoid rounding errors @@ -268,9 +279,11 @@ def __set_position(self, data: PoseStamped, min_diff=0.001): # check if the new position is valid dist = self.__dist_to(data.pose.position) if dist < min_diff: - self.logdebug("New position disregarded, " - f"as dist ({round(dist, 3)}) to current pos " - f"< min_diff ({round(min_diff, 3)})") + self.logdebug( + "New position disregarded, " + f"as dist ({round(dist, 3)}) to current pos " + f"< min_diff ({round(min_diff, 3)})" + ) return new_x = data.pose.position.x @@ -296,7 +309,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('stanley_controller', args=args) + roscomp.init("stanley_controller", args=args) try: node = StanleyController() @@ -307,5 +320,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index e97bc1c8..90aa0f68 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -21,41 +21,45 @@ class VehicleController(CompatibleNode): """ def __init__(self): - super(VehicleController, self).__init__('vehicle_controller') - self.loginfo('VehicleController node started') - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) - self.role_name = self.get_param('role_name', 'ego_vehicle') + super(VehicleController, self).__init__("vehicle_controller") + self.loginfo("VehicleController node started") + self.control_loop_rate = self.get_param("control_loop_rate", 0.05) + self.role_name = self.get_param("role_name", "ego_vehicle") self.__curr_behavior = None # only unstuck behavior is relevant here # Publisher for Carla Vehicle Control Commands self.control_publisher: Publisher = self.new_publisher( CarlaEgoVehicleControl, - f'/carla/{self.role_name}/vehicle_control_cmd', - qos_profile=10) + f"/carla/{self.role_name}/vehicle_control_cmd", + qos_profile=10, + ) # Publisher for Status TODO: Maybe unneccessary self.status_pub: Publisher = self.new_publisher( Bool, f"/carla/{self.role_name}/status", qos_profile=QoSProfile( - depth=1, - durability=DurabilityPolicy.TRANSIENT_LOCAL)) + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), + ) # Publisher for which steering-controller is mainly used # 1 = PurePursuit and 2 = Stanley self.controller_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/controller", - qos_profile=QoSProfile(depth=10, - durability=DurabilityPolicy.TRANSIENT_LOCAL) + qos_profile=QoSProfile( + depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), ) self.emergency_pub: Publisher = self.new_publisher( Bool, f"/paf/{self.role_name}/emergency", - qos_profile=QoSProfile(depth=10, - durability=DurabilityPolicy.TRANSIENT_LOCAL) + qos_profile=QoSProfile( + depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), ) # Subscribers @@ -63,51 +67,53 @@ def __init__(self): String, f"/paf/{self.role_name}/curr_behavior", self.__set_curr_behavior, - qos_profile=1) + qos_profile=1, + ) self.emergency_sub: Subscriber = self.new_subscription( Bool, f"/paf/{self.role_name}/emergency", self.__set_emergency, - qos_profile=QoSProfile(depth=10, - durability=DurabilityPolicy.TRANSIENT_LOCAL) + qos_profile=QoSProfile( + depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), ) self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__get_velocity, - qos_profile=1) + qos_profile=1, + ) self.throttle_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/throttle", self.__set_throttle, - qos_profile=1) + qos_profile=1, + ) self.brake_sub: Subscriber = self.new_subscription( - Float32, - f"/paf/{self.role_name}/brake", - self.__set_brake, - qos_profile=1) + Float32, f"/paf/{self.role_name}/brake", self.__set_brake, qos_profile=1 + ) self.reverse_sub: Subscriber = self.new_subscription( - Bool, - f"/paf/{self.role_name}/reverse", - self.__set_reverse, - qos_profile=1) + Bool, f"/paf/{self.role_name}/reverse", self.__set_reverse, qos_profile=1 + ) self.pure_pursuit_steer_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/pure_pursuit_steer", self.__set_pure_pursuit_steer, - qos_profile=1) + qos_profile=1, + ) self.stanley_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/stanley_steer", self.__set_stanley_steer, - qos_profile=1) + qos_profile=1, + ) self.__reverse: bool = False self.__emergency: bool = False @@ -123,7 +129,7 @@ def run(self): :return: """ self.status_pub.publish(True) - self.loginfo('VehicleController node running') + self.loginfo("VehicleController node running") def loop(timer_event=None) -> None: """ @@ -143,8 +149,10 @@ def loop(timer_event=None) -> None: steer = self._s_steer else: # while doing the unstuck routine we don't want to steer - if self.__curr_behavior == "us_unstuck" or \ - self.__curr_behavior == "us_stop": + if ( + self.__curr_behavior == "us_unstuck" + or self.__curr_behavior == "us_stop" + ): steer = 0 else: steer = self._p_steer @@ -157,8 +165,7 @@ def loop(timer_event=None) -> None: message.throttle = self.__throttle message.brake = self.__brake message.steer = steer - message.header.stamp = roscomp.ros_timestamp(self.get_time(), - from_sec=True) + message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) self.control_publisher.publish(message) self.new_timer(self.control_loop_rate, loop) @@ -204,8 +211,7 @@ def __emergency_brake(self, active) -> None: message.reverse = True message.hand_brake = True message.manual_gear_shift = False - message.header.stamp = roscomp.ros_timestamp(self.get_time(), - from_sec=True) + message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) else: self.__emergency = False message.throttle = 0 @@ -214,8 +220,7 @@ def __emergency_brake(self, active) -> None: message.reverse = False message.hand_brake = False message.manual_gear_shift = False - message.header.stamp = roscomp.ros_timestamp(self.get_time(), - from_sec=True) + message.header.stamp = roscomp.ros_timestamp(self.get_time(), from_sec=True) self.control_publisher.publish(message) def __get_velocity(self, data: CarlaSpeedometer) -> None: @@ -230,11 +235,12 @@ def __get_velocity(self, data: CarlaSpeedometer) -> None: return if data.speed < 0.1: # vehicle has come to a stop self.__emergency_brake(False) - self.loginfo("Emergency breaking disengaged " - "(Emergency breaking has been executed successfully)") + self.loginfo( + "Emergency breaking disengaged " + "(Emergency breaking has been executed successfully)" + ) for _ in range(7): # publish 7 times just to be safe - self.emergency_pub.publish( - Bool(False)) + self.emergency_pub.publish(Bool(False)) def __set_throttle(self, data): self.__throttle = data.data @@ -246,12 +252,12 @@ def __set_reverse(self, data): self.__reverse = data.data def __set_pure_pursuit_steer(self, data: Float32): - r = (math.pi / 2) # convert from RAD to [-1;1] - self._p_steer = (data.data / r) + r = math.pi / 2 # convert from RAD to [-1;1] + self._p_steer = data.data / r def __set_stanley_steer(self, data: Float32): - r = (math.pi / 2) # convert from RAD to [-1;1] - self._s_steer = (data.data / r) + r = math.pi / 2 # convert from RAD to [-1;1] + self._s_steer = data.data / r def main(args=None): @@ -259,7 +265,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('vehicle_controller', args=args) + roscomp.init("vehicle_controller", args=args) try: node = VehicleController() @@ -270,5 +276,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index bf43ab41..db1f53aa 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -15,38 +15,37 @@ class VelocityController(CompatibleNode): """ def __init__(self): - super(VelocityController, self).__init__('velocity_controller') - self.loginfo('VelocityController node started') + super(VelocityController, self).__init__("velocity_controller") + self.loginfo("VelocityController node started") - self.control_loop_rate = self.get_param('control_loop_rate', 0.05) - self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param("control_loop_rate", 0.05) + self.role_name = self.get_param("role_name", "ego_vehicle") self.target_velocity_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/target_velocity", self.__get_target_velocity, - qos_profile=1) + qos_profile=1, + ) self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__get_current_velocity, - qos_profile=1) + qos_profile=1, + ) self.throttle_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/throttle", - qos_profile=1) + Float32, f"/paf/{self.role_name}/throttle", qos_profile=1 + ) self.brake_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/brake", - qos_profile=1) + Float32, f"/paf/{self.role_name}/brake", qos_profile=1 + ) self.reverse_pub: Publisher = self.new_publisher( - Bool, - f"/paf/{self.role_name}/reverse", - qos_profile=1) + Bool, f"/paf/{self.role_name}/reverse", qos_profile=1 + ) self.__current_velocity: float = None self.__target_velocity: float = None @@ -56,7 +55,7 @@ def run(self): Starts the main loop of the node :return: """ - self.loginfo('VelocityController node running') + self.loginfo("VelocityController node running") # PID for throttle pid_t = PID(0.60, 0.00076, 0.63) # since we use this for braking aswell, allow -1 to 0. @@ -71,15 +70,19 @@ def loop(timer_event=None): :return: """ if self.__target_velocity is None: - self.logdebug("VelocityController hasn't received target" - "_velocity yet. target_velocity has been set to" - "default value 0") + self.logdebug( + "VelocityController hasn't received target" + "_velocity yet. target_velocity has been set to" + "default value 0" + ) self.__target_velocity = 0 if self.__current_velocity is None: - self.logdebug("VelocityController hasn't received " - "current_velocity yet and can therefore not" - "publish a throttle value") + self.logdebug( + "VelocityController hasn't received " + "current_velocity yet and can therefore not" + "publish a throttle value" + ) return if self.__target_velocity < 0: @@ -135,7 +138,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('velocity_controller', args=args) + roscomp.init("velocity_controller", args=args) try: node = VelocityController() @@ -146,5 +149,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/agent/setup.py b/code/agent/setup.py index ad7850df..0b6d7399 100644 --- a/code/agent/setup.py +++ b/code/agent/setup.py @@ -2,6 +2,5 @@ from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup(packages=['agent'], - package_dir={'': 'src'}) +setup_args = generate_distutils_setup(packages=["agent"], package_dir={"": "src"}) setup(**setup_args) diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index 5a97786b..a57f2d6a 100755 --- a/code/agent/src/agent/agent.py +++ b/code/agent/src/agent/agent.py @@ -4,7 +4,7 @@ def get_entry_point(): - return 'PAFAgent' + return "PAFAgent" class PAFAgent(ROS1Agent): @@ -14,76 +14,102 @@ def setup(self, path_to_conf_file): def get_ros_entrypoint(self): return { - 'package': 'agent', - 'launch_file': 'agent.launch', - 'parameters': { - 'role_name': 'hero', - } + "package": "agent", + "launch_file": "agent.launch", + "parameters": { + "role_name": "hero", + }, } def sensors(self): sensors = [ { - 'type': 'sensor.camera.rgb', - 'id': 'Center', - 'x': 0.0, 'y': 0.0, 'z': 1.70, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, - 'width': 1280, 'height': 720, 'fov': 100 - }, + "type": "sensor.camera.rgb", + "id": "Center", + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "width": 1280, + "height": 720, + "fov": 100, + }, { - 'type': 'sensor.camera.rgb', - 'id': 'Back', - 'x': 0.0, 'y': 0.0, 'z': 1.70, - 'roll': 0.0, 'pitch': 0.0, 'yaw': math.radians(180.0), - 'width': 1280, 'height': 720, 'fov': 100 - }, + "type": "sensor.camera.rgb", + "id": "Back", + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": math.radians(180.0), + "width": 1280, + "height": 720, + "fov": 100, + }, { - 'type': 'sensor.camera.rgb', - 'id': 'Left', - 'x': 0.0, 'y': 0.0, 'z': 1.70, - 'roll': 0.0, 'pitch': 0.0, 'yaw': math.radians(-90.0), - 'width': 1280, 'height': 720, 'fov': 100 - }, + "type": "sensor.camera.rgb", + "id": "Left", + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": math.radians(-90.0), + "width": 1280, + "height": 720, + "fov": 100, + }, { - 'type': 'sensor.camera.rgb', - 'id': 'Right', - 'x': 0.0, 'y': 0.0, 'z': 1.70, - 'roll': 0.0, 'pitch': 0.0, 'yaw': math.radians(90.0), - 'width': 1280, 'height': 720, 'fov': 100 - }, + "type": "sensor.camera.rgb", + "id": "Right", + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": math.radians(90.0), + "width": 1280, + "height": 720, + "fov": 100, + }, { - 'type': 'sensor.lidar.ray_cast', - 'id': 'LIDAR', - 'x': 0.0, 'y': 0.0, 'z': 1.70, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - }, + "type": "sensor.lidar.ray_cast", + "id": "LIDAR", + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + }, { - 'type': 'sensor.other.radar', - 'id': 'RADAR', - 'x': 2.0, 'y': 0.0, 'z': 0.7, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0, - 'horizontal_fov': 30, 'vertical_fov': 30 - }, + "type": "sensor.other.radar", + "id": "RADAR", + "x": 2.0, + "y": 0.0, + "z": 0.7, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + "horizontal_fov": 30, + "vertical_fov": 30, + }, + {"type": "sensor.other.gnss", "id": "GPS", "x": 0.0, "y": 0.0, "z": 0.0}, { - 'type': 'sensor.other.gnss', - 'id': 'GPS', - 'x': 0.0, 'y': 0.0, 'z': 0.0 - }, - { - 'type': 'sensor.other.imu', - 'id': 'IMU', - 'x': 0.0, 'y': 0.0, 'z': 0.0, - 'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0 - }, - { - 'type': 'sensor.opendrive_map', - 'id': 'OpenDRIVE', - 'reading_frequency': 1 - }, - { - 'type': 'sensor.speedometer', - 'id': 'Speed' - } + "type": "sensor.other.imu", + "id": "IMU", + "x": 0.0, + "y": 0.0, + "z": 0.0, + "roll": 0.0, + "pitch": 0.0, + "yaw": 0.0, + }, + {"type": "sensor.opendrive_map", "id": "OpenDRIVE", "reading_frequency": 1}, + {"type": "sensor.speedometer", "id": "Speed"}, ] return sensors diff --git a/code/mock/setup.py b/code/mock/setup.py index 8f232a1f..bf698614 100755 --- a/code/mock/setup.py +++ b/code/mock/setup.py @@ -1,6 +1,5 @@ from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup(packages=['mock'], - package_dir={'': 'src'}) +setup_args = generate_distutils_setup(packages=["mock"], package_dir={"": "src"}) setup(**setup_args) diff --git a/code/mock/src/mock_intersection_clear.py b/code/mock/src/mock_intersection_clear.py index df7cea62..bb584941 100755 --- a/code/mock/src/mock_intersection_clear.py +++ b/code/mock/src/mock_intersection_clear.py @@ -10,18 +10,17 @@ class MockIntersectionClearPublisher(CompatibleNode): This node publishes intersection clear information. It can be used for testing. """ + def __init__(self): - super(MockIntersectionClearPublisher, self).\ - __init__('intersectionClearMock') + super(MockIntersectionClearPublisher, self).__init__("intersectionClearMock") - self.control_loop_rate = self.get_param('control_loop_rate', 10) - self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param("control_loop_rate", 10) + self.role_name = self.get_param("role_name", "ego_vehicle") # self.enabled = self.get_param('enabled', False) self.stop_sign_pub: Publisher = self.new_publisher( - Bool, - f"/paf/{self.role_name}/intersection_clear", - qos_profile=1) + Bool, f"/paf/{self.role_name}/intersection_clear", qos_profile=1 + ) self.delta = 0.2 self.distance = 75.0 self.isClear = False @@ -33,7 +32,7 @@ def run(self): """ # if not self.enabled: # return - self.loginfo('Stopsignmock node running') + self.loginfo("Stopsignmock node running") def loop(timer_event=None): """ @@ -47,6 +46,7 @@ def loop(timer_event=None): if self.distance < 0.0: self.isClear = True self.stop_sign_pub.publish(msg) + self.new_timer(self.control_loop_rate, loop) self.spin() @@ -56,7 +56,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('velocity_publisher_dummy', args=args) + roscomp.init("velocity_publisher_dummy", args=args) try: node = MockIntersectionClearPublisher() @@ -67,5 +67,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/mock/src/mock_stop_sign.py b/code/mock/src/mock_stop_sign.py index a5ec4691..f85f6344 100755 --- a/code/mock/src/mock_stop_sign.py +++ b/code/mock/src/mock_stop_sign.py @@ -2,6 +2,7 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Publisher + # from std_msgs.msg import Float32 from mock.msg import Stop_sign @@ -11,18 +12,17 @@ class MockStopSignPublisher(CompatibleNode): This node publishes stop sign light information. It can be used for testing. """ + def __init__(self): - super(MockStopSignPublisher, self).\ - __init__('stopSignMock') + super(MockStopSignPublisher, self).__init__("stopSignMock") - self.control_loop_rate = self.get_param('control_loop_rate', 10) - self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param("control_loop_rate", 10) + self.role_name = self.get_param("role_name", "ego_vehicle") # self.enabled = self.get_param('enabled', False) self.stop_sign_pub: Publisher = self.new_publisher( - Stop_sign, - f"/paf/{self.role_name}/stop_sign", - qos_profile=1) + Stop_sign, f"/paf/{self.role_name}/stop_sign", qos_profile=1 + ) self.delta = 0.2 self.distance = 20.0 self.isStop = False @@ -34,7 +34,7 @@ def run(self): """ # if not self.enabled: # return - self.loginfo('Stopsignmock node running') + self.loginfo("Stopsignmock node running") def loop(timer_event=None): """ @@ -50,6 +50,7 @@ def loop(timer_event=None): self.distance = 20.0 msg.distance = self.distance self.stop_sign_pub.publish(msg) + self.new_timer(self.control_loop_rate, loop) self.spin() @@ -59,7 +60,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('velocity_publisher_dummy', args=args) + roscomp.init("velocity_publisher_dummy", args=args) try: node = MockStopSignPublisher() @@ -70,5 +71,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/mock/src/mock_traffic_light.py b/code/mock/src/mock_traffic_light.py index 13852e03..63ff289a 100755 --- a/code/mock/src/mock_traffic_light.py +++ b/code/mock/src/mock_traffic_light.py @@ -2,6 +2,7 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Publisher + # from std_msgs.msg import Float32 from mock.msg import Traffic_light @@ -10,18 +11,17 @@ class MockTrafficLightPublisher(CompatibleNode): """ This node publishes traffic light information. It can be used for testing. """ + def __init__(self): - super(MockTrafficLightPublisher, self).\ - __init__('trafficLightMock') + super(MockTrafficLightPublisher, self).__init__("trafficLightMock") - self.control_loop_rate = self.get_param('control_loop_rate', 10) - self.role_name = self.get_param('role_name', 'ego_vehicle') + self.control_loop_rate = self.get_param("control_loop_rate", 10) + self.role_name = self.get_param("role_name", "ego_vehicle") # self.enabled = self.get_param('enabled', False) self.traffic_light_pub: Publisher = self.new_publisher( - Traffic_light, - f"/paf/{self.role_name}/traffic_light", - qos_profile=1) + Traffic_light, f"/paf/{self.role_name}/traffic_light", qos_profile=1 + ) self.delta = 0.2 self.distance = 20.0 self.color = "green" @@ -33,7 +33,7 @@ def run(self): """ # if not self.enabled: # return - self.loginfo('TrafficLightmock node running') + self.loginfo("TrafficLightmock node running") def loop(timer_event=None): """ @@ -54,6 +54,7 @@ def loop(timer_event=None): self.distance = 20.0 msg.distance = self.distance self.traffic_light_pub.publish(msg) + self.new_timer(self.control_loop_rate, loop) self.spin() @@ -63,7 +64,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('traffic_light_publisher_dummy', args=args) + roscomp.init("traffic_light_publisher_dummy", args=args) try: node = MockTrafficLightPublisher() @@ -74,5 +75,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/perception/setup.py b/code/perception/setup.py index 13c6ef29..91c01e49 100644 --- a/code/perception/setup.py +++ b/code/perception/setup.py @@ -2,6 +2,5 @@ from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup(packages=['perception'], - package_dir={'': 'src'}) +setup_args = generate_distutils_setup(packages=["perception"], package_dir={"": "src"}) setup(**setup_args) diff --git a/code/perception/src/coordinate_transformation.py b/code/perception/src/coordinate_transformation.py index 4f062770..b6bf33e0 100755 --- a/code/perception/src/coordinate_transformation.py +++ b/code/perception/src/coordinate_transformation.py @@ -7,9 +7,11 @@ A good source to read up on the different reference frames is: http://dirsig.cis.rit.edu/docs/new/coordinates.html """ + import math import numpy as np from scipy.spatial.transform import Rotation + # from tf.transformations import euler_from_quaternion @@ -55,12 +57,14 @@ def geodetic_to_enu(lat, lon, alt): scale = math.cos(CoordinateTransformer.la_ref * math.pi / 180.0) basex = scale * math.pi * a / 180.0 * CoordinateTransformer.ln_ref - basey = scale * a * math.log( - math.tan((90.0 + CoordinateTransformer.la_ref) * math.pi / 360.0)) + basey = ( + scale + * a + * math.log(math.tan((90.0 + CoordinateTransformer.la_ref) * math.pi / 360.0)) + ) x = scale * math.pi * a / 180.0 * lon - basex - y = scale * a * math.log( - math.tan((90.0 + lat) * math.pi / 360.0)) - basey + y = scale * a * math.log(math.tan((90.0 + lat) * math.pi / 360.0)) - basey # Is not necessary in new version # y *= -1 @@ -140,6 +144,7 @@ def quat_to_heading(quaternion): return heading + # old functions # def quat_to_heading(msg): # orientation_q = msg diff --git a/code/perception/src/dataset_converter.py b/code/perception/src/dataset_converter.py index 1122f981..9976878f 100755 --- a/code/perception/src/dataset_converter.py +++ b/code/perception/src/dataset_converter.py @@ -9,25 +9,21 @@ def create_argparse(): - argparser = ArgumentParser( - description='CARLA Dataset Converter') + argparser = ArgumentParser(description="CARLA Dataset Converter") + argparser.add_argument("input_dir", help="Path to the input directory") + argparser.add_argument("output_dir", help="Path to the output directory") argparser.add_argument( - 'input_dir', - help='Path to the input directory') - argparser.add_argument( - 'output_dir', - help='Path to the output directory') - argparser.add_argument( - '--force', + "--force", default=False, - action='store_true', - help='Overwrite output if already exists') + action="store_true", + help="Overwrite output if already exists", + ) argparser.add_argument( - '--shuffle', + "--shuffle", default=False, - action='store_true', - help='Shuffle the dataset before splitting it' - ' into train, test and validation sets' + action="store_true", + help="Shuffle the dataset before splitting it" + " into train, test and validation sets", ) return argparser @@ -64,35 +60,35 @@ def main(): output_dir.mkdir() else: raise ValueError( - f"given output_dir ({output_dir.as_posix()}) already exists!") + f"given output_dir ({output_dir.as_posix()}) already exists!" + ) if not input_dir.is_dir(): - raise ValueError( - f"input_dir ({input_dir.as_posix()}) needs to be a directory") + raise ValueError(f"input_dir ({input_dir.as_posix()}) needs to be a directory") # first create the necessary directories - groundtruth = output_dir / 'groundtruth' + groundtruth = output_dir / "groundtruth" groundtruth.mkdir(parents=True) rgb_files = {} instance_files = {} # populate dicts - for file in input_dir.rglob('*.png'): + for file in input_dir.rglob("*.png"): side = file.parts[-2] - if 'rgb' in file.parts: + if "rgb" in file.parts: add_to_side_list(rgb_files, side, file) - if 'instance' in file.parts: + if "instance" in file.parts: add_to_side_list(instance_files, side, file) # sort images according to their sequence number for side in rgb_files: - rgb_files[side] = sorted(rgb_files[side], - key=lambda path: int(path.stem)) - instance_files[side] = sorted(instance_files[side], - key=lambda path: int(path.stem)) + rgb_files[side] = sorted(rgb_files[side], key=lambda path: int(path.stem)) + instance_files[side] = sorted( + instance_files[side], key=lambda path: int(path.stem) + ) - print(f'rgb_files[{side}] length: {len(rgb_files[side])}') - print(f'instance_files[{side}] length: {len(instance_files[side])}') + print(f"rgb_files[{side}] length: {len(rgb_files[side])}") + print(f"instance_files[{side}] length: {len(instance_files[side])}") splits = [train, test, val] split_names = ["train", "test", "val"] @@ -128,9 +124,8 @@ def main(): # convert_image_to_cityscapes_labelids( # instance_image, # groundtruth_target_dir / instance_file_name) zzzyy - copyfile(instance_image, - groundtruth_target_dir / instance_file_name) + copyfile(instance_image, groundtruth_target_dir / instance_file_name) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/perception/src/dataset_generator.py b/code/perception/src/dataset_generator.py index fd849d11..0a52c637 100755 --- a/code/perception/src/dataset_generator.py +++ b/code/perception/src/dataset_generator.py @@ -9,8 +9,8 @@ from threading import Thread # get carla host and port from environment variables -CARLA_HOST = os.environ.get('CARLA_HOST', 'localhost') -CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) +CARLA_HOST = os.environ.get("CARLA_HOST", "localhost") +CARLA_PORT = int(os.environ.get("CARLA_PORT", "2000")) def destroy_actors(actors): @@ -25,13 +25,13 @@ def setup_empty_world(client): world.wait_for_tick() # destroy all actors - destroy_actors(world.get_actors().filter('vehicle.*')) - destroy_actors(world.get_actors().filter('walker.*')) - destroy_actors(world.get_actors().filter('controller.*')) + destroy_actors(world.get_actors().filter("vehicle.*")) + destroy_actors(world.get_actors().filter("walker.*")) + destroy_actors(world.get_actors().filter("controller.*")) # spawn ego vehicle blueprint_library = world.get_blueprint_library() - bp = blueprint_library.filter('vehicle.*')[0] + bp = blueprint_library.filter("vehicle.*")[0] ego_vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) ego_vehicle.set_autopilot(True) @@ -39,8 +39,10 @@ def setup_empty_world(client): spectator = world.get_spectator() # set spectator to follow ego vehicle with offset spectator.set_transform( - carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), - carla.Rotation(pitch=-90))) + carla.Transform( + ego_vehicle.get_location() + carla.Location(z=50), carla.Rotation(pitch=-90) + ) + ) # create traffic manager traffic_manager = client.get_trafficmanager(8000) @@ -50,32 +52,30 @@ def setup_empty_world(client): blueprint_library = world.get_blueprint_library() count = 0 while count < 14: - bp = choice(blueprint_library.filter('walker.pedestrian.*')) + bp = choice(blueprint_library.filter("walker.pedestrian.*")) spawn_point = carla.Transform() spawn_point.location = world.get_random_location_from_navigation() - traffic_pedestrian = world.try_spawn_actor(bp, - spawn_point) + traffic_pedestrian = world.try_spawn_actor(bp, spawn_point) if traffic_pedestrian is None: continue - controller_bp = blueprint_library.find('controller.ai.walker') - ai_controller = world.try_spawn_actor(controller_bp, carla.Transform(), - traffic_pedestrian) + controller_bp = blueprint_library.find("controller.ai.walker") + ai_controller = world.try_spawn_actor( + controller_bp, carla.Transform(), traffic_pedestrian + ) ai_controller.start() - ai_controller.go_to_location( - world.get_random_location_from_navigation()) + ai_controller.go_to_location(world.get_random_location_from_navigation()) ai_controller.set_max_speed(1.0) count += 1 # spawn traffic vehicles for i in range(18): - bp = choice(blueprint_library.filter('vehicle.*')) - traffic_vehicle = world.spawn_actor(bp, - world.get_map().get_spawn_points()[ - i + 1]) - traffic_manager.vehicle_percentage_speed_difference(traffic_vehicle, - 0.0) + bp = choice(blueprint_library.filter("vehicle.*")) + traffic_vehicle = world.spawn_actor( + bp, world.get_map().get_spawn_points()[i + 1] + ) + traffic_manager.vehicle_percentage_speed_difference(traffic_vehicle, 0.0) traffic_vehicle.set_autopilot(True) return ego_vehicle @@ -94,13 +94,13 @@ def __init__(self, output_dir): "center": Queue(), "right": Queue(), "back": Queue(), - "left": Queue() + "left": Queue(), } self.instance_camera_queues = { "center": Queue(), "right": Queue(), "back": Queue(), - "left": Queue() + "left": Queue(), } def save_image(self, image, dir): @@ -112,39 +112,37 @@ def save_segmented_image(self, image, dir): def setup_cameras(self, world, ego_vehicle): # get instance segmentation camera blueprint instance_camera_bp = world.get_blueprint_library().find( - 'sensor.camera.instance_segmentation' + "sensor.camera.instance_segmentation" ) # get camera blueprint - camera_bp = world.get_blueprint_library().find('sensor.camera.rgb') - camera_bp.set_attribute('sensor_tick', '1.0') + camera_bp = world.get_blueprint_library().find("sensor.camera.rgb") + camera_bp.set_attribute("sensor_tick", "1.0") # set leaderboard attributes - camera_bp.set_attribute('lens_circle_multiplier', '0.0') - camera_bp.set_attribute('lens_circle_falloff', '5.0') - camera_bp.set_attribute('chromatic_aberration_intensity', '0.5') - camera_bp.set_attribute('chromatic_aberration_offset', '0.0') + camera_bp.set_attribute("lens_circle_multiplier", "0.0") + camera_bp.set_attribute("lens_circle_falloff", "5.0") + camera_bp.set_attribute("chromatic_aberration_intensity", "0.5") + camera_bp.set_attribute("chromatic_aberration_offset", "0.0") IMAGE_WIDTH = 1280 IMAGE_HEIGHT = 720 # set resolution - camera_bp.set_attribute('image_size_x', str(IMAGE_WIDTH)) - camera_bp.set_attribute('image_size_y', str(IMAGE_HEIGHT)) + camera_bp.set_attribute("image_size_x", str(IMAGE_WIDTH)) + camera_bp.set_attribute("image_size_y", str(IMAGE_HEIGHT)) - instance_camera_bp.set_attribute('sensor_tick', '1.0') + instance_camera_bp.set_attribute("sensor_tick", "1.0") # set resolution - instance_camera_bp.set_attribute('image_size_x', str(IMAGE_WIDTH)) - instance_camera_bp.set_attribute('image_size_y', str(IMAGE_HEIGHT)) + instance_camera_bp.set_attribute("image_size_x", str(IMAGE_WIDTH)) + instance_camera_bp.set_attribute("image_size_y", str(IMAGE_HEIGHT)) - camera_init_transform = carla.Transform( - carla.Location(z=1.7) - ) + camera_init_transform = carla.Transform(carla.Location(z=1.7)) for i, direction in enumerate(self.directions): print("Creating camera {}".format(direction)) - camera_bp.set_attribute('role_name', direction) - instance_camera_bp.set_attribute('role_name', direction) + camera_bp.set_attribute("role_name", direction) + instance_camera_bp.set_attribute("role_name", direction) # add rotation to camera transform camera_init_transform.rotation.yaw = i * 90 # create camera @@ -152,20 +150,19 @@ def setup_cameras(self, world, ego_vehicle): camera_bp, camera_init_transform, attach_to=ego_vehicle, - attachment_type=carla.AttachmentType.Rigid + attachment_type=carla.AttachmentType.Rigid, ) # create instance segmentation camera instance_camera = world.spawn_actor( instance_camera_bp, camera_init_transform, attach_to=ego_vehicle, - attachment_type=carla.AttachmentType.Rigid + attachment_type=carla.AttachmentType.Rigid, ) - camera.listen( - lambda image, dir=direction: self.save_image(image, dir)) + camera.listen(lambda image, dir=direction: self.save_image(image, dir)) instance_camera.listen( - lambda image, dir=direction: self.save_segmented_image( - image, dir)) + lambda image, dir=direction: self.save_segmented_image(image, dir) + ) self.cameras.append(camera) self.instance_cameras.append(instance_camera) @@ -179,17 +176,11 @@ def save_images_worker(self, direction, stop_event): image = image_queue.get() if counter < 2500: image.save_to_disk( - '{}/rgb/{}/{}.png'.format( - output_dir, direction, - counter - ) + "{}/rgb/{}/{}.png".format(output_dir, direction, counter) ) instance_image = instance_image_queue.get() instance_image.save_to_disk( - '{}/instance/{}/{}.png'.format( - output_dir, direction, - counter - ) + "{}/instance/{}/{}.png".format(output_dir, direction, counter) ) counter += 1 @@ -199,8 +190,9 @@ def start_saving_images(self): for direction in self.directions: thread_stop_event = threading.Event() self.thread_stop_events.append(thread_stop_event) - t = Thread(target=self.save_images_worker, - args=(direction, thread_stop_event)) + t = Thread( + target=self.save_images_worker, args=(direction, thread_stop_event) + ) self.threads.append(t) t.start() @@ -211,53 +203,51 @@ def stop_saving_images(self): t.join() -def find_ego_vehicle(world, role_name='hero'): - if world.get_actors().filter('vehicle.*'): +def find_ego_vehicle(world, role_name="hero"): + if world.get_actors().filter("vehicle.*"): # get ego vehicle with hero role - for actor in world.get_actors().filter('vehicle.*'): - if actor.attributes['role_name'] == role_name: + for actor in world.get_actors().filter("vehicle.*"): + if actor.attributes["role_name"] == role_name: return actor def create_argparse(): - argparser = argparse.ArgumentParser( - description='CARLA Dataset Generator') - argparser.add_argument( - '--output-dir', - metavar='DIR', - default='output', - help='Path to the output directory') + argparser = argparse.ArgumentParser(description="CARLA Dataset Generator") argparser.add_argument( - '--host', - metavar='H', - default='localhost', - help='host of the carla server' + "--output-dir", + metavar="DIR", + default="output", + help="Path to the output directory", ) argparser.add_argument( - '--port', - metavar='P', - default=2000, - type=int, - help='port of the carla server' + "--host", metavar="H", default="localhost", help="host of the carla server" ) argparser.add_argument( - '--use-empty-world', - action='store_true', - help='set up an empty world and spawn ego vehicle', - default=False + "--port", metavar="P", default=2000, type=int, help="port of the carla server" ) argparser.add_argument( - '--town', - metavar='T', - default='Town12', - help='town to load' + "--use-empty-world", + action="store_true", + help="set up an empty world and spawn ego vehicle", + default=False, ) + argparser.add_argument("--town", metavar="T", default="Town12", help="town to load") return argparser -if __name__ == '__main__': - towns = {"Town01", "Town02", "Town03", "Town04", "Town05", "Town06", - "Town07", "Town10", "Town11", "Town12"} +if __name__ == "__main__": + towns = { + "Town01", + "Town02", + "Town03", + "Town04", + "Town05", + "Town06", + "Town07", + "Town10", + "Town11", + "Town12", + } argparser = create_argparse() args = argparser.parse_args() town = args.town @@ -280,7 +270,7 @@ def create_argparse(): ego_vehicle = find_ego_vehicle(world) if not ego_vehicle: - raise RuntimeError('No vehicle found in the world') + raise RuntimeError("No vehicle found in the world") dataset_generator = DatasetGenerator(output_dir) dataset_generator.setup_cameras(world, ego_vehicle) diff --git a/code/perception/src/experiments/Position_Heading_Datasets/viz.py b/code/perception/src/experiments/Position_Heading_Datasets/viz.py index bee260b8..decdd2ca 100644 --- a/code/perception/src/experiments/Position_Heading_Datasets/viz.py +++ b/code/perception/src/experiments/Position_Heading_Datasets/viz.py @@ -27,7 +27,8 @@ # region PLOTS -def plot_best_tuned_file_by_type(type='x', error_type='MSE', check_type='IQR'): + +def plot_best_tuned_file_by_type(type="x", error_type="MSE", check_type="IQR"): """ Calculates the best tuned file by type and error type using a specific check type. @@ -52,37 +53,34 @@ def plot_best_tuned_file_by_type(type='x', error_type='MSE', check_type='IQR'): else: file_name = "data_" + str(i) + ".csv" - ideal, test_filter, current, unfiltered = ( - get_x_or_y_or_h_from_csv_file(file_name, type)) + ideal, test_filter, current, unfiltered = get_x_or_y_or_h_from_csv_file( + file_name, type + ) # calculate the error for each method by error_type - if error_type == 'MSE': + if error_type == "MSE": # Calculate the MSE for each method val_test_filter, test_filter_list = calculate_mse_x_or_y_or_h( - ideal, - test_filter) - val_current, current_list = calculate_mse_x_or_y_or_h( - ideal, - current) + ideal, test_filter + ) + val_current, current_list = calculate_mse_x_or_y_or_h(ideal, current) val_unfiltered, unfiltered_list = calculate_mse_x_or_y_or_h( - ideal, - unfiltered) - elif error_type == 'MAE': + ideal, unfiltered + ) + elif error_type == "MAE": # Calculate the MAE for each method val_test_filter, test_filter_list = calculate_mae_x_or_y_or_h( - ideal, - test_filter) - val_current, current_list = calculate_mae_x_or_y_or_h( - ideal, - current) + ideal, test_filter + ) + val_current, current_list = calculate_mae_x_or_y_or_h(ideal, current) val_unfiltered, unfiltered_list = calculate_mae_x_or_y_or_h( - ideal, - unfiltered) + ideal, unfiltered + ) vals = [val_test_filter, val_current, val_unfiltered] # evaluate the best tuned file by check_type - if check_type == 'IQR': + if check_type == "IQR": q3, q1 = np.percentile(test_filter_list, [80, 0]) iqr = q3 - q1 if i == FILE_START: @@ -95,7 +93,7 @@ def plot_best_tuned_file_by_type(type='x', error_type='MSE', check_type='IQR'): if abs(iqr) < abs(best_val): best_val = iqr best_file = file_name - elif check_type == 'default': + elif check_type == "default": if i == FILE_START: best_val = vals[0] best_file = file_name @@ -114,7 +112,7 @@ def plot_best_tuned_file_by_type(type='x', error_type='MSE', check_type='IQR'): return -def plot_x_or_y_or_h_notched_box(file_name, type='x', error_type='MSE'): +def plot_x_or_y_or_h_notched_box(file_name, type="x", error_type="MSE"): """ Calculates and plots the error of x, y or heading data for any given error type. @@ -127,38 +125,33 @@ def plot_x_or_y_or_h_notched_box(file_name, type='x', error_type='MSE'): Returns: """ - if type == 'x': - ideal, test_filter, current, unfiltered = ( - get_x_or_y_or_h_from_csv_file(file_name, 'x')) - elif type == 'y': - ideal, test_filter, current, unfiltered = ( - get_x_or_y_or_h_from_csv_file(file_name, 'y')) - elif type == 'h': - ideal, test_filter, current, unfiltered = ( - get_x_or_y_or_h_from_csv_file(file_name, 'h')) - - if error_type == 'MSE': + if type == "x": + ideal, test_filter, current, unfiltered = get_x_or_y_or_h_from_csv_file( + file_name, "x" + ) + elif type == "y": + ideal, test_filter, current, unfiltered = get_x_or_y_or_h_from_csv_file( + file_name, "y" + ) + elif type == "h": + ideal, test_filter, current, unfiltered = get_x_or_y_or_h_from_csv_file( + file_name, "h" + ) + + if error_type == "MSE": # Calculate the MSE for each method val_test_filter, test_filter_list = calculate_mse_x_or_y_or_h( - ideal, - test_filter) - val_current, current_list = calculate_mse_x_or_y_or_h( - ideal, - current) - val_unfiltered, unfiltered_list = calculate_mse_x_or_y_or_h( - ideal, - unfiltered) - elif error_type == 'MAE': + ideal, test_filter + ) + val_current, current_list = calculate_mse_x_or_y_or_h(ideal, current) + val_unfiltered, unfiltered_list = calculate_mse_x_or_y_or_h(ideal, unfiltered) + elif error_type == "MAE": # Calculate the MAE for each method val_test_filter, test_filter_list = calculate_mae_x_or_y_or_h( - ideal, - test_filter) - val_current, current_list = calculate_mae_x_or_y_or_h( - ideal, - current) - val_unfiltered, unfiltered_list = calculate_mae_x_or_y_or_h( - ideal, - unfiltered) + ideal, test_filter + ) + val_current, current_list = calculate_mae_x_or_y_or_h(ideal, current) + val_unfiltered, unfiltered_list = calculate_mae_x_or_y_or_h(ideal, unfiltered) # Create a new figure fig, ax = plt.subplots() @@ -166,37 +159,53 @@ def plot_x_or_y_or_h_notched_box(file_name, type='x', error_type='MSE'): # Create a list of all errors error_list = [test_filter_list, current_list, unfiltered_list] # Create a box plot with notches - boxplot = ax.boxplot(error_list, notch=True, - labels=['Test Filter', 'Current', 'Unfiltered'], - patch_artist=True) + boxplot = ax.boxplot( + error_list, + notch=True, + labels=["Test Filter", "Current", "Unfiltered"], + patch_artist=True, + ) # fill with colors and put median vals in the boxes - colors = ['pink', 'lightblue', 'lightgreen'] + colors = ["pink", "lightblue", "lightgreen"] - tuple = zip(boxplot['boxes'], colors, boxplot['medians'], - boxplot['whiskers'][::2]) + tuple = zip(boxplot["boxes"], colors, boxplot["medians"], boxplot["whiskers"][::2]) for i, (box, color, median, whiskers) in enumerate(tuple): box.set_facecolor(color) median_val = median.get_ydata()[1] - ax.text(i+1, median_val, f'Median: {median_val:.2f}', va='center', - ha='center', backgroundcolor='white') + ax.text( + i + 1, + median_val, + f"Median: {median_val:.2f}", + va="center", + ha="center", + backgroundcolor="white", + ) # Calculate IQR q3, q1 = np.percentile(error_list[i], [75, 0]) iqr = q3 - q1 # Get the y position for the IQR text - median_y = boxplot['medians'][i].get_ydata()[0] # height of the notch + median_y = boxplot["medians"][i].get_ydata()[0] # height of the notch # Add the IQR text - ax.text(i+0.8, median_y, f'IQR: {iqr:.2f}', va='center', - ha='center', rotation=90, color='red', backgroundcolor='white') + ax.text( + i + 0.8, + median_y, + f"IQR: {iqr:.2f}", + va="center", + ha="center", + rotation=90, + color="red", + backgroundcolor="white", + ) # Set the labels - ax.set_xlabel('Filter') + ax.set_xlabel("Filter") ax.set_ylabel(error_type) - ax.set_title(error_type + ' of ' + type + ' for different methods') + ax.set_title(error_type + " of " + type + " for different methods") ax.yaxis.grid(True) # Show the plot @@ -215,15 +224,13 @@ def plot_MSE_notched_box(file_name): float: Mean Squared Error (MSE). """ ideal_pos, test_filter_pos, current_pos, unfiltered_pos = ( - get_positions_from_csv_file(file_name)) + get_positions_from_csv_file(file_name) + ) # Calculate the MSE for each method - val_test_filter, test_filter_list = calculate_mse_pos(ideal_pos, - test_filter_pos) - val_current, current_list = calculate_mse_pos(ideal_pos, - current_pos) - val_unfiltered, unfiltered_list = calculate_mse_pos(ideal_pos, - unfiltered_pos) + val_test_filter, test_filter_list = calculate_mse_pos(ideal_pos, test_filter_pos) + val_current, current_list = calculate_mse_pos(ideal_pos, current_pos) + val_unfiltered, unfiltered_list = calculate_mse_pos(ideal_pos, unfiltered_pos) # Create a new figure fig, ax = plt.subplots() @@ -231,23 +238,33 @@ def plot_MSE_notched_box(file_name): # Create a list of all positions pos_mse_list = [test_filter_list, current_list, unfiltered_list] # Create a box plot with notches - boxplot = ax.boxplot(pos_mse_list, notch=True, - labels=['Test Filter', 'Current', 'Unfiltered'], - patch_artist=True) + boxplot = ax.boxplot( + pos_mse_list, + notch=True, + labels=["Test Filter", "Current", "Unfiltered"], + patch_artist=True, + ) # fill with colors and put median vals in the boxes - colors = ['pink', 'lightblue', 'lightgreen'] - for i, (box, color, median) in enumerate(zip(boxplot['boxes'], colors, - boxplot['medians'])): + colors = ["pink", "lightblue", "lightgreen"] + for i, (box, color, median) in enumerate( + zip(boxplot["boxes"], colors, boxplot["medians"]) + ): box.set_facecolor(color) median_val = median.get_ydata()[1] - ax.text(i+1, median_val, f'Median: {median_val:.2f}', va='center', - ha='center', backgroundcolor='white') + ax.text( + i + 1, + median_val, + f"Median: {median_val:.2f}", + va="center", + ha="center", + backgroundcolor="white", + ) # Set the labels - ax.set_xlabel('Method') - ax.set_ylabel('MSE') - ax.set_title('MSE for different methods') + ax.set_xlabel("Method") + ax.set_ylabel("MSE") + ax.set_title("MSE for different methods") ax.yaxis.grid(True) # Show the plot @@ -266,15 +283,13 @@ def plot_MAE_notched_box(file_name): float: Mean Absolute Error (MAE). """ ideal_pos, test_filter_pos, current_pos, unfiltered_pos = ( - get_positions_from_csv_file(file_name)) + get_positions_from_csv_file(file_name) + ) # Calculate the MAE for each method - mae_test_filter, test_filter_list = calculate_mae_pos(ideal_pos, - test_filter_pos) - mae_current, current_list = calculate_mae_pos(ideal_pos, - current_pos) - mae_unfiltered, unfiltered_list = calculate_mae_pos(ideal_pos, - unfiltered_pos) + mae_test_filter, test_filter_list = calculate_mae_pos(ideal_pos, test_filter_pos) + mae_current, current_list = calculate_mae_pos(ideal_pos, current_pos) + mae_unfiltered, unfiltered_list = calculate_mae_pos(ideal_pos, unfiltered_pos) # Create a new figure fig, ax = plt.subplots() @@ -283,23 +298,33 @@ def plot_MAE_notched_box(file_name): pos_mae_list = [test_filter_list, current_list, unfiltered_list] # Create a box plot with notches - boxplot = ax.boxplot(pos_mae_list, notch=True, - labels=['Test Filter', 'Current', 'Unfiltered'], - patch_artist=True) + boxplot = ax.boxplot( + pos_mae_list, + notch=True, + labels=["Test Filter", "Current", "Unfiltered"], + patch_artist=True, + ) # fill with colors and put median vals in the boxes - colors = ['pink', 'lightblue', 'lightgreen'] - for i, (box, color, median) in enumerate(zip(boxplot['boxes'], colors, - boxplot['medians'])): + colors = ["pink", "lightblue", "lightgreen"] + for i, (box, color, median) in enumerate( + zip(boxplot["boxes"], colors, boxplot["medians"]) + ): box.set_facecolor(color) median_val = median.get_ydata()[1] - ax.text(i+1, median_val, f'Median: {median_val:.2f}', va='center', - ha='center', backgroundcolor='white') + ax.text( + i + 1, + median_val, + f"Median: {median_val:.2f}", + va="center", + ha="center", + backgroundcolor="white", + ) # Set the labels - ax.set_xlabel('Method') - ax.set_ylabel('MAE') - ax.set_title('MAE for different methods') + ax.set_xlabel("Method") + ax.set_ylabel("MAE") + ax.set_title("MAE for different methods") ax.yaxis.grid(True) # Show the plot @@ -327,31 +352,30 @@ def plot_CEP(file_name): df_y.set_index(df_y.columns[0], inplace=True) # create pos tuples of the x and y data and store them as numpy arrays - ideal_pos = np.array(list(zip(df_x['Ideal (Carla)'], - df_y['Ideal (Carla)']))) - test_filter_pos = np.array(list(zip(df_x['Test Filter'], - df_y['Test Filter']))) - current_pos = np.array(list(zip(df_x['Current'], - df_y['Current']))) - unfiltered_pos = np.array(list(zip(df_x['Unfiltered'], - df_y['Unfiltered']))) + ideal_pos = np.array(list(zip(df_x["Ideal (Carla)"], df_y["Ideal (Carla)"]))) + test_filter_pos = np.array(list(zip(df_x["Test Filter"], df_y["Test Filter"]))) + current_pos = np.array(list(zip(df_x["Current"], df_y["Current"]))) + unfiltered_pos = np.array(list(zip(df_x["Unfiltered"], df_y["Unfiltered"]))) # create CEP for each method cep_test_filter, cep_current, cep_unfiltered = calculate_cep( - ideal_pos, test_filter_pos, current_pos, unfiltered_pos) + ideal_pos, test_filter_pos, current_pos, unfiltered_pos + ) # plot the cep as error circles of different colors in the x-y plane # Create a new figure fig, ax = plt.subplots() # Create circles with the given radii - circle_test_filter = plt.Circle((0, 0), cep_test_filter, fill=False, - label='Test Filter', - color='r') - circle_current = plt.Circle((0, 0), cep_current, fill=False, - label='Current', color='g') - circle_unfiltered = plt.Circle((0, 0), cep_unfiltered, fill=False, - label='Unfiltered', color='b') + circle_test_filter = plt.Circle( + (0, 0), cep_test_filter, fill=False, label="Test Filter", color="r" + ) + circle_current = plt.Circle( + (0, 0), cep_current, fill=False, label="Current", color="g" + ) + circle_unfiltered = plt.Circle( + (0, 0), cep_unfiltered, fill=False, label="Unfiltered", color="b" + ) # Add the circles to the plot ax.add_artist(circle_test_filter) @@ -359,10 +383,14 @@ def plot_CEP(file_name): ax.add_artist(circle_unfiltered) # Set the limits of the plot to show all circles - ax.set_xlim(-max(cep_test_filter, cep_current, cep_unfiltered), - max(cep_test_filter, cep_current, cep_unfiltered)) - ax.set_ylim(-max(cep_test_filter, cep_current, cep_unfiltered), - max(cep_test_filter, cep_current, cep_unfiltered)) + ax.set_xlim( + -max(cep_test_filter, cep_current, cep_unfiltered), + max(cep_test_filter, cep_current, cep_unfiltered), + ) + ax.set_ylim( + -max(cep_test_filter, cep_current, cep_unfiltered), + max(cep_test_filter, cep_current, cep_unfiltered), + ) # Add a legend plt.legend() @@ -371,33 +399,33 @@ def plot_CEP(file_name): plt.grid(True) # Set the y-axis label to 'Distance in Meters' - plt.ylabel('Distance in Meters') + plt.ylabel("Distance in Meters") # Set the x-axis label to 'Distance in Meters' - plt.xlabel('Distance in Meters') + plt.xlabel("Distance in Meters") -def plot_csv_x_or_y(file_name, type='x'): +def plot_csv_x_or_y(file_name, type="x"): """ Plots the x or y data from a CSV file. Parameters: file_name (str): The name of the CSV file. """ - if type == 'x': + if type == "x": file_path = folder_path_x + file_name - elif type == 'y': + elif type == "y": file_path = folder_path_y + file_name # Read the CSV file into a DataFrame df = pd.read_csv(file_path) # Plot the 'test_filter' (blue) and 'current' (green) - plt.plot(df['Test Filter'], 'b-', label='Test Filter') - plt.plot(df['Current'], 'g-', label='Current') + plt.plot(df["Test Filter"], "b-", label="Test Filter") + plt.plot(df["Current"], "g-", label="Current") # Plot the 'ideal' column with a red dotted line - plt.plot(df['Ideal (Carla)'], 'r:', label='Ideal') + plt.plot(df["Ideal (Carla)"], "r:", label="Ideal") # Display the legend plt.legend() @@ -409,12 +437,12 @@ def plot_csv_x_or_y(file_name, type='x'): # Set the y- # axis label to 'Distance in Meters' - plt.ylabel('Distance in Meters') + plt.ylabel("Distance in Meters") # Set the x-axis label to 'Time' - plt.xlabel('Time in seconds') + plt.xlabel("Time in seconds") - plt.title(type + ' Positions in Meters') + plt.title(type + " Positions in Meters") def plot_csv_heading(file_name): @@ -432,11 +460,11 @@ def plot_csv_heading(file_name): # Plot the 'test_filter_heading' (blue) and 'current_heading' (green) # line style - plt.plot(df['Test Filter'], 'b-', label='Test Filter Heading') - plt.plot(df['Current'], 'g-', label='Current Heading') + plt.plot(df["Test Filter"], "b-", label="Test Filter Heading") + plt.plot(df["Current"], "g-", label="Current Heading") # Plot the 'ideal_heading' column with a blue dotted line - plt.plot(df['Ideal (Carla)'], 'r:', label='Ideal Heading') + plt.plot(df["Ideal (Carla)"], "r:", label="Ideal Heading") # Display the legend plt.legend() # Plot the DataFrame @@ -446,10 +474,10 @@ def plot_csv_heading(file_name): plt.grid(True) # Set the y-axis label to 'Radians' - plt.ylabel('Heading in Radians') + plt.ylabel("Heading in Radians") # Set the x-axis label to 'Time' - plt.xlabel('Time in seconds') + plt.xlabel("Time in seconds") def plot_csv_positions(file_name): @@ -462,22 +490,19 @@ def plot_csv_positions(file_name): Returns: """ # Read the CSV file into a DataFrame - ideal, test_filter, current, unfiltered = ( - get_positions_from_csv_file(file_name)) + ideal, test_filter, current, unfiltered = get_positions_from_csv_file(file_name) ideal_x, ideal_y = zip(*ideal) test_filter_x, test_filter_y = zip(test_filter) current_x, current_y = zip(current) unfiltered_x, unfiltered_y = zip(unfiltered) - plt.plot(ideal_x, ideal_y, marker=',', - color='red', label='Ideal') - plt.plot(test_filter_x, test_filter_y, marker='.', - color='blue', label='Test Filter') - plt.plot(current_x, current_y, marker='.', - color='green', label='Current') - plt.plot(unfiltered_x, unfiltered_y, marker='.', - color='purple', label='Unfiltered') + plt.plot(ideal_x, ideal_y, marker=",", color="red", label="Ideal") + plt.plot( + test_filter_x, test_filter_y, marker=".", color="blue", label="Test Filter" + ) + plt.plot(current_x, current_y, marker=".", color="green", label="Current") + plt.plot(unfiltered_x, unfiltered_y, marker=".", color="purple", label="Unfiltered") # Display the legend plt.legend() @@ -486,12 +511,14 @@ def plot_csv_positions(file_name): plt.grid(True) # Set the y-axis label to 'Y Position in Meters' - plt.ylabel('Y Position in Meters') + plt.ylabel("Y Position in Meters") # Set the x-axis label to 'X Position in Meters' - plt.xlabel('X Position in Meters') + plt.xlabel("X Position in Meters") + + plt.title("X and Y Positions in Meters") + - plt.title('X and Y Positions in Meters') # endregion PLOTS @@ -528,7 +555,7 @@ def calculate_mse_pos(ideal, estimated): Tuple: A tuple containing the MSE and the error for each position. """ # Calculate the errors - error = np.linalg.norm(ideal - estimated, axis=1)**2 + error = np.linalg.norm(ideal - estimated, axis=1) ** 2 # Calculate the MSE mse = np.mean(error) @@ -570,7 +597,7 @@ def calculate_mse_x_or_y_or_h(ideal, estimated): or heading. """ # Calculate the errors - error = (ideal - estimated)**2 + error = (ideal - estimated) ** 2 # Calculate the MSE mse = np.mean(error) @@ -597,9 +624,9 @@ def calculate_cep(ideal, test_filter, current, unfiltered, percentile=90): tuple: A tuple containing the CEP for each method. """ # Calculate the errors - error_test_filter = np.sqrt(np.sum((test_filter - ideal)**2, axis=1)) - error_current = np.sqrt(np.sum((current - ideal)**2, axis=1)) - error_unfiltered = np.sqrt(np.sum((unfiltered - ideal)**2, axis=1)) + error_test_filter = np.sqrt(np.sum((test_filter - ideal) ** 2, axis=1)) + error_current = np.sqrt(np.sum((current - ideal) ** 2, axis=1)) + error_unfiltered = np.sqrt(np.sum((unfiltered - ideal) ** 2, axis=1)) # Calculate the CEP for each method cep_test_filter = np.percentile(error_test_filter, percentile) @@ -608,6 +635,7 @@ def calculate_cep(ideal, test_filter, current, unfiltered, percentile=90): return cep_test_filter, cep_current, cep_unfiltered + # endregion CALCUATIONS @@ -642,19 +670,15 @@ def get_positions_from_csv_file(file_name, file_name_y=file_name): df_y.set_index(df_y.columns[0], inplace=True) # create pos tuples of the x and y data and store them as numpy arrays - ideal_pos = np.array(list(zip(df_x['Ideal (Carla)'], - df_y['Ideal (Carla)']))) - test_filter_pos = np.array(list(zip(df_x['Test Filter'], - df_y['Test Filter']))) - current_pos = np.array(list(zip(df_x['Current'], - df_y['Current']))) - unfiltered_pos = np.array(list(zip(df_x['Unfiltered'], - df_y['Unfiltered']))) + ideal_pos = np.array(list(zip(df_x["Ideal (Carla)"], df_y["Ideal (Carla)"]))) + test_filter_pos = np.array(list(zip(df_x["Test Filter"], df_y["Test Filter"]))) + current_pos = np.array(list(zip(df_x["Current"], df_y["Current"]))) + unfiltered_pos = np.array(list(zip(df_x["Unfiltered"], df_y["Unfiltered"]))) return ideal_pos, test_filter_pos, current_pos, unfiltered_pos -def get_x_or_y_or_h_from_csv_file(file_name, type='x'): +def get_x_or_y_or_h_from_csv_file(file_name, type="x"): """ Reads x,y or heading data from CSV files and returns them as numpy arrays. @@ -669,13 +693,13 @@ def get_x_or_y_or_h_from_csv_file(file_name, type='x'): - test_filter: The data estimated using test_filter filtering. - current: The data calculated using a running average. - - unfiltered: The unfiltered data. """ + - unfiltered: The unfiltered data.""" - if type == 'x': + if type == "x": file_path = folder_path_x + file_name - elif type == 'y': + elif type == "y": file_path = folder_path_y + file_name - elif type == 'h': + elif type == "h": file_path = folder_path_heading + file_name # Read the CSV file into a DataFrame @@ -685,26 +709,28 @@ def get_x_or_y_or_h_from_csv_file(file_name, type='x'): # Set the first column (time) as the index of the DataFrames df.set_index(df.columns[0], inplace=True) - if type == 'x': + if type == "x": # store x as numpy arrays - ideal = np.array(df['Ideal (Carla)']) - test_filter = np.array(df['Test Filter']) - current = np.array(df['Current']) - unfiltered = np.array(df['Unfiltered']) - elif type == 'y': + ideal = np.array(df["Ideal (Carla)"]) + test_filter = np.array(df["Test Filter"]) + current = np.array(df["Current"]) + unfiltered = np.array(df["Unfiltered"]) + elif type == "y": # store y as numpy arrays - ideal = np.array(df['Ideal (Carla)']) - test_filter = np.array(df['Test Filter']) - current = np.array(df['Current']) - unfiltered = np.array(df['Unfiltered']) - elif type == 'h': + ideal = np.array(df["Ideal (Carla)"]) + test_filter = np.array(df["Test Filter"]) + current = np.array(df["Current"]) + unfiltered = np.array(df["Unfiltered"]) + elif type == "h": # store heading as numpy arrays - ideal = np.array(df['Ideal (Carla)']) - test_filter = np.array(df['Test Filter']) - current = np.array(df['Current']) - unfiltered = np.array(df['Unfiltered']) + ideal = np.array(df["Ideal (Carla)"]) + test_filter = np.array(df["Test Filter"]) + current = np.array(df["Current"]) + unfiltered = np.array(df["Unfiltered"]) return ideal, test_filter, current, unfiltered + + # endregion helper methods @@ -714,11 +740,11 @@ def get_x_or_y_or_h_from_csv_file(file_name, type='x'): data = file_name plot_CEP(data) - plot_x_or_y_or_h_notched_box(data, type='x', error_type='MSE') - plot_x_or_y_or_h_notched_box(data, type='x', error_type='MAE') + plot_x_or_y_or_h_notched_box(data, type="x", error_type="MSE") + plot_x_or_y_or_h_notched_box(data, type="x", error_type="MAE") - plot_x_or_y_or_h_notched_box(data, type='y', error_type='MSE') - plot_x_or_y_or_h_notched_box(data, type='y', error_type='MAE') + plot_x_or_y_or_h_notched_box(data, type="y", error_type="MSE") + plot_x_or_y_or_h_notched_box(data, type="y", error_type="MAE") # plot_x_or_y_or_h_notched_box(data, type='h', error_type='MSE') # plot_x_or_y_or_h_notched_box(data, type='h', error_type='MAE') diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 4c53ca71..6601c514 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -8,6 +8,7 @@ from perception.msg import Waypoint, LaneChange import math + # import rospy @@ -24,8 +25,7 @@ def __init__(self): :return: """ - super(GlobalPlanDistance, self).__init__('global_plan_distance' - '_publisher') + super(GlobalPlanDistance, self).__init__("global_plan_distance" "_publisher") self.loginfo("GlobalPlanDistance node started") # basic info @@ -42,23 +42,25 @@ def __init__(self): PoseStamped, "/paf/" + self.role_name + "/current_pos", self.update_position, - qos_profile=1) + qos_profile=1, + ) self.global_plan_subscriber = self.new_subscription( CarlaRoute, "/carla/" + self.role_name + "/global_plan", self.update_global_route, - qos_profile=1) + qos_profile=1, + ) self.waypoint_publisher = self.new_publisher( - Waypoint, - "/paf/" + self.role_name + "/waypoint_distance", - qos_profile=1) + Waypoint, "/paf/" + self.role_name + "/waypoint_distance", qos_profile=1 + ) self.lane_change_publisher = self.new_publisher( LaneChange, "/paf/" + self.role_name + "/lane_change_distance", - qos_profile=1) + qos_profile=1, + ) def update_position(self, pos): """ @@ -78,35 +80,38 @@ def distance(a, b): # points to navigate to if self.global_route is not None and self.global_route: - current_distance = distance(self.global_route[0].position, - self.current_pos.position) - next_distance = distance(self.global_route[1].position, - self.current_pos.position) + current_distance = distance( + self.global_route[0].position, self.current_pos.position + ) + next_distance = distance( + self.global_route[1].position, self.current_pos.position + ) # if the road option indicates an intersection, the distance to the # next waypoint is also the distance to the stop line if self.road_options[0] < 4: # print("publish waypoint") - self.waypoint_publisher.publish( - Waypoint(current_distance, True)) + self.waypoint_publisher.publish(Waypoint(current_distance, True)) self.lane_change_publisher.publish( - LaneChange(current_distance, False, self.road_options[0])) + LaneChange(current_distance, False, self.road_options[0]) + ) else: - self.waypoint_publisher.publish( - Waypoint(current_distance, False)) + self.waypoint_publisher.publish(Waypoint(current_distance, False)) if self.road_options[0] == 5 or self.road_options[0] == 6: self.lane_change_publisher.publish( - LaneChange(current_distance, True, - self.road_options[0])) + LaneChange(current_distance, True, self.road_options[0]) + ) # if we reached the next waypoint, pop it and the next point will # be published if current_distance < 2.5 or next_distance < current_distance: self.road_options.pop(0) self.global_route.pop(0) - if self.road_options[0] in {5, 6} and \ - self.road_options[0] == self.road_options[1]: + if ( + self.road_options[0] in {5, 6} + and self.road_options[0] == self.road_options[1] + ): self.road_options[1] = 4 print(f"next road option = {self.road_options[0]}") diff --git a/code/perception/src/kalman_filter.py b/code/perception/src/kalman_filter.py index 6a44bc22..c5370750 100755 --- a/code/perception/src/kalman_filter.py +++ b/code/perception/src/kalman_filter.py @@ -16,7 +16,7 @@ GPS_RUNNING_AVG_ARGS = 10 -''' +""" For more information see the documentation in: ../../doc/perception/kalman_filter.md @@ -67,7 +67,7 @@ self.Q = np.diag([0.0001, 0.0001, 0.00001, 0.00001, 0.000001, 0.00001]) The measurement covariance matrix R is defined as: self.R = np.diag([0.0007, 0.0007, 0, 0, 0, 0]) -''' +""" class KalmanFilter(CompatibleNode): @@ -77,14 +77,15 @@ class KalmanFilter(CompatibleNode): For more information see the documentation in: ../../doc/perception/kalman_filter.md """ + def __init__(self): """ Constructor / Setup :return: """ - super(KalmanFilter, self).__init__('kalman_filter_node') + super(KalmanFilter, self).__init__("kalman_filter_node") - self.loginfo('KalmanFilter node started') + self.loginfo("KalmanFilter node started") # basic info self.transformer = None # for coordinate transformation self.role_name = self.get_param("role_name", "hero") @@ -97,7 +98,7 @@ def __init__(self): self.initialized = False # state vector X - ''' + """ [ [initial_x], [initial_y], @@ -106,7 +107,7 @@ def __init__(self): [yaw], [omega_z], ] - ''' + """ self.x_est = np.zeros((6, 1)) # estimated state vector self.P_est = np.zeros((6, 6)) # estiamted state covariance matrix @@ -115,7 +116,7 @@ def __init__(self): self.P_pred = np.zeros((6, 6)) # Predicted state covariance matrix # Define state transition matrix - ''' + """ # [x ... ] # [y ... ] # [v_x ... ] @@ -128,27 +129,35 @@ def __init__(self): v_y = v_y yaw = yaw + omega_z * dt omega_z = omega_z - ''' - self.A = np.array([[1, 0, self.dt, 0, 0, 0], - [0, 1, 0, self.dt, 0, 0], - [0, 0, 1, 0, 0, 0], - [0, 0, 0, 1, 0, 0], - [0, 0, 0, 0, 1, self.dt], - [0, 0, 0, 0, 0, 1]]) + """ + self.A = np.array( + [ + [1, 0, self.dt, 0, 0, 0], + [0, 1, 0, self.dt, 0, 0], + [0, 0, 1, 0, 0, 0], + [0, 0, 0, 1, 0, 0], + [0, 0, 0, 0, 1, self.dt], + [0, 0, 0, 0, 0, 1], + ] + ) # Define measurement matrix - ''' + """ 1. GPS: x, y 2. Velocity: v_x, v_y 3. IMU: yaw, omega_z -> 6 measurements for a state vector of 6 - ''' - self.H = np.array([[1, 0, 0, 0, 0, 0], # x - [0, 1, 0, 0, 0, 0], # y - [0, 0, 1, 0, 0, 0], # v_x - [0, 0, 0, 1, 0, 0], # v_y - [0, 0, 0, 0, 1, 0], # yaw - [0, 0, 0, 0, 0, 1]]) # omega_z + """ + self.H = np.array( + [ + [1, 0, 0, 0, 0, 0], # x + [0, 1, 0, 0, 0, 0], # y + [0, 0, 1, 0, 0, 0], # v_x + [0, 0, 0, 1, 0, 0], # v_y + [0, 0, 0, 0, 1, 0], # yaw + [0, 0, 0, 0, 0, 1], + ] + ) # omega_z # Define Measurement Variables self.z_gps = np.zeros((2, 1)) # GPS measurements (x, y) @@ -167,25 +176,28 @@ def __init__(self): self.latitude = 0 # latitude of the current position - # Subscriber + # Subscriber # Initialize the subscriber for the OpenDrive Map self.map_sub = self.new_subscription( String, "/carla/" + self.role_name + "/OpenDRIVE", self.get_geoRef, - qos_profile=1) + qos_profile=1, + ) # Initialize the subscriber for the IMU Data self.imu_subscriber = self.new_subscription( Imu, "/carla/" + self.role_name + "/IMU", self.update_imu_data, - qos_profile=1) + qos_profile=1, + ) # Initialize the subscriber for the GPS Data self.gps_subscriber = self.new_subscription( NavSatFix, "/carla/" + self.role_name + "/GPS", self.update_gps_data, - qos_profile=1) + qos_profile=1, + ) # Initialize the subscriber for the unfiltered_pos in XYZ self.avg_z = np.zeros((GPS_RUNNING_AVG_ARGS, 1)) self.avg_gps_counter: int = 0 @@ -193,25 +205,25 @@ def __init__(self): PoseStamped, "/paf/" + self.role_name + "/unfiltered_pos", self.update_unfiltered_pos, - qos_profile=1) + qos_profile=1, + ) # Initialize the subscriber for the velocity self.velocity_subscriber = self.new_subscription( CarlaSpeedometer, "/carla/" + self.role_name + "/Speed", self.update_velocity, - qos_profile=1) + qos_profile=1, + ) - # Publisher + # Publisher # Initialize the publisher for the kalman-position self.kalman_position_publisher = self.new_publisher( - PoseStamped, - "/paf/" + self.role_name + "/kalman_pos", - qos_profile=1) + PoseStamped, "/paf/" + self.role_name + "/kalman_pos", qos_profile=1 + ) # Initialize the publisher for the kalman-heading self.kalman_heading_publisher = self.new_publisher( - Float32, - "/paf/" + self.role_name + "/kalman_heading", - qos_profile=1) + Float32, "/paf/" + self.role_name + "/kalman_heading", qos_profile=1 + ) def run(self): """ @@ -223,16 +235,20 @@ def run(self): rospy.sleep(1) rospy.sleep(1) - self.loginfo('KalmanFilter started its loop!') + self.loginfo("KalmanFilter started its loop!") # initialize the state vector x_est and the covariance matrix P_est # initial state vector x_0 - self.x_0 = np.array([[self.z_gps[0, 0]], - [self.z_gps[1, 0]], - [self.z_v[0, 0]], - [self.z_v[1, 0]], - [self.z_imu[0, 0]], - [self.z_imu[1, 0]]]) + self.x_0 = np.array( + [ + [self.z_gps[0, 0]], + [self.z_gps[1, 0]], + [self.z_v[0, 0]], + [self.z_v[1, 0]], + [self.z_imu[0, 0]], + [self.z_imu[1, 0]], + ] + ) self.x_est = np.copy(self.x_0) # estimated initial state vector self.P_est = np.eye(6) * 1 # estiamted initialstatecovariancematrix @@ -331,10 +347,12 @@ def update_imu_data(self, imu_data): orientation_w = imu_data.orientation.w # Calculate the heading based on the orientation given by the IMU - data_orientation_q = [orientation_x, - orientation_y, - orientation_z, - orientation_w] + data_orientation_q = [ + orientation_x, + orientation_y, + orientation_z, + orientation_w, + ] heading = quat_to_heading(data_orientation_q) @@ -417,8 +435,8 @@ def get_geoRef(self, opendrive: String): indexLatEnd = geoRefText.find(" ", indexLat) indexLonEnd = geoRefText.find(" ", indexLon) - latValue = float(geoRefText[indexLat + len(latString):indexLatEnd]) - lonValue = float(geoRefText[indexLon + len(lonString):indexLonEnd]) + latValue = float(geoRefText[indexLat + len(latString) : indexLatEnd]) + lonValue = float(geoRefText[indexLon + len(lonString) : indexLonEnd]) CoordinateTransformer.la_ref = latValue CoordinateTransformer.ln_ref = lonValue @@ -431,7 +449,7 @@ def main(args=None): Main function starts the node :param args: """ - roscomp.init('kalman_filter_node', args=args) + roscomp.init("kalman_filter_node", args=args) try: node = KalmanFilter() @@ -442,5 +460,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index f5f7c964..04394ee2 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -4,20 +4,22 @@ import numpy as np import lidar_filter_utility from sensor_msgs.msg import PointCloud2 + # from mpl_toolkits.mplot3d import Axes3D # from itertools import combinations from sensor_msgs.msg import Image as ImageMsg from cv_bridge import CvBridge + # from matplotlib.colors import LinearSegmentedColormap -class LidarDistance(): - """ See doc/perception/lidar_distance_utility.md on - how to configute this node +class LidarDistance: + """See doc/perception/lidar_distance_utility.md on + how to configute this node """ def callback(self, data): - """ Callback function, filters a PontCloud2 message + """Callback function, filters a PontCloud2 message by restrictions defined in the launchfile. Publishes a Depth image for the specified camera angle. @@ -32,22 +34,21 @@ def callback(self, data): reconstruct_bit_mask_center = lidar_filter_utility.bounding_box( coordinates, max_x=np.inf, - min_x=0., + min_x=0.0, min_z=-1.6, ) - reconstruct_coordinates_center = \ - coordinates[reconstruct_bit_mask_center] + reconstruct_coordinates_center = coordinates[reconstruct_bit_mask_center] reconstruct_coordinates_xyz_center = np.array( lidar_filter_utility.remove_field_name( - reconstruct_coordinates_center, - 'intensity') - .tolist() + reconstruct_coordinates_center, "intensity" + ).tolist() ) dist_array_center = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_center, focus="Center") - dist_array_center_msg = \ - self.bridge.cv2_to_imgmsg(dist_array_center, - encoding="passthrough") + reconstruct_coordinates_xyz_center, focus="Center" + ) + dist_array_center_msg = self.bridge.cv2_to_imgmsg( + dist_array_center, encoding="passthrough" + ) dist_array_center_msg.header = data.header self.dist_array_center_publisher.publish(dist_array_center_msg) @@ -61,15 +62,15 @@ def callback(self, data): reconstruct_coordinates_back = coordinates[reconstruct_bit_mask_back] reconstruct_coordinates_xyz_back = np.array( lidar_filter_utility.remove_field_name( - reconstruct_coordinates_back, - 'intensity') - .tolist() + reconstruct_coordinates_back, "intensity" + ).tolist() ) dist_array_back = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_back, focus="Back") - dist_array_back_msg = \ - self.bridge.cv2_to_imgmsg(dist_array_back, - encoding="passthrough") + reconstruct_coordinates_xyz_back, focus="Back" + ) + dist_array_back_msg = self.bridge.cv2_to_imgmsg( + dist_array_back, encoding="passthrough" + ) dist_array_back_msg.header = data.header self.dist_array_back_publisher.publish(dist_array_back_msg) @@ -83,37 +84,34 @@ def callback(self, data): reconstruct_coordinates_left = coordinates[reconstruct_bit_mask_left] reconstruct_coordinates_xyz_left = np.array( lidar_filter_utility.remove_field_name( - reconstruct_coordinates_left, - 'intensity') - .tolist() + reconstruct_coordinates_left, "intensity" + ).tolist() ) dist_array_left = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_left, focus="Left") - dist_array_left_msg = \ - self.bridge.cv2_to_imgmsg(dist_array_left, - encoding="passthrough") + reconstruct_coordinates_xyz_left, focus="Left" + ) + dist_array_left_msg = self.bridge.cv2_to_imgmsg( + dist_array_left, encoding="passthrough" + ) dist_array_left_msg.header = data.header self.dist_array_left_publisher.publish(dist_array_left_msg) # Right reconstruct_bit_mask_right = lidar_filter_utility.bounding_box( - coordinates, - max_y=-0.0, - min_y=-np.inf, - min_z=-1.6 + coordinates, max_y=-0.0, min_y=-np.inf, min_z=-1.6 ) reconstruct_coordinates_right = coordinates[reconstruct_bit_mask_right] reconstruct_coordinates_xyz_right = np.array( lidar_filter_utility.remove_field_name( - reconstruct_coordinates_right, - 'intensity') - .tolist() + reconstruct_coordinates_right, "intensity" + ).tolist() ) dist_array_right = self.reconstruct_img_from_lidar( - reconstruct_coordinates_xyz_right, focus="Right") - dist_array_right_msg = \ - self.bridge.cv2_to_imgmsg(dist_array_right, - encoding="passthrough") + reconstruct_coordinates_xyz_right, focus="Right" + ) + dist_array_right_msg = self.bridge.cv2_to_imgmsg( + dist_array_right, encoding="passthrough" + ) dist_array_right_msg.header = data.header self.dist_array_right_publisher.publish(dist_array_right_msg) @@ -122,60 +120,51 @@ def listener(self): Initializes the node and it's publishers """ # run simultaneously. - rospy.init_node('lidar_distance') + rospy.init_node("lidar_distance") self.bridge = CvBridge() self.pub_pointcloud = rospy.Publisher( rospy.get_param( - '~point_cloud_topic', - '/carla/hero/' + rospy.get_namespace() + '_filtered' + "~point_cloud_topic", + "/carla/hero/" + rospy.get_namespace() + "_filtered", ), PointCloud2, - queue_size=10 + queue_size=10, ) # publisher for dist_array self.dist_array_center_publisher = rospy.Publisher( - rospy.get_param( - '~image_distance_topic', - '/paf/hero/Center/dist_array' - ), + rospy.get_param("~image_distance_topic", "/paf/hero/Center/dist_array"), ImageMsg, - queue_size=10 + queue_size=10, ) # publisher for dist_array self.dist_array_back_publisher = rospy.Publisher( - rospy.get_param( - '~image_distance_topic', - '/paf/hero/Back/dist_array' - ), + rospy.get_param("~image_distance_topic", "/paf/hero/Back/dist_array"), ImageMsg, - queue_size=10 + queue_size=10, ) # publisher for dist_array self.dist_array_left_publisher = rospy.Publisher( - rospy.get_param( - '~image_distance_topic', - '/paf/hero/Left/dist_array' - ), + rospy.get_param("~image_distance_topic", "/paf/hero/Left/dist_array"), ImageMsg, - queue_size=10 + queue_size=10, ) # publisher for dist_array self.dist_array_right_publisher = rospy.Publisher( - rospy.get_param( - '~image_distance_topic', - '/paf/hero/Right/dist_array' - ), + rospy.get_param("~image_distance_topic", "/paf/hero/Right/dist_array"), ImageMsg, - queue_size=10 + queue_size=10, ) - rospy.Subscriber(rospy.get_param('~source_topic', "/carla/hero/LIDAR"), - PointCloud2, self.callback) + rospy.Subscriber( + rospy.get_param("~source_topic", "/carla/hero/LIDAR"), + PointCloud2, + self.callback, + ) rospy.spin() @@ -214,45 +203,49 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): if focus == "Center": point = np.array([c[1], c[2], c[0], 1]) pixel = np.matmul(m, point) - x, y = int(pixel[0]/pixel[2]), int(pixel[1]/pixel[2]) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) if x >= 0 and x <= 1280 and y >= 0 and y <= 720: - img[719-y][1279-x] = c[0] - dist_array[719-y][1279-x] = \ - np.array([c[0], c[1], c[2]], dtype=np.float32) + img[719 - y][1279 - x] = c[0] + dist_array[719 - y][1279 - x] = np.array( + [c[0], c[1], c[2]], dtype=np.float32 + ) # back depth image if focus == "Back": point = np.array([c[1], c[2], c[0], 1]) pixel = np.matmul(m, point) - x, y = int(pixel[0]/pixel[2]), int(pixel[1]/pixel[2]) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) if x >= 0 and x <= 1280 and y >= 0 and y < 720: - img[y][1279-x] = -c[0] - dist_array[y][1279-x] = \ - np.array([-c[0], c[1], c[2]], dtype=np.float32) + img[y][1279 - x] = -c[0] + dist_array[y][1279 - x] = np.array( + [-c[0], c[1], c[2]], dtype=np.float32 + ) # left depth image if focus == "Left": point = np.array([c[0], c[2], c[1], 1]) pixel = np.matmul(m, point) - x, y = int(pixel[0]/pixel[2]), int(pixel[1]/pixel[2]) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) if x >= 0 and x <= 1280 and y >= 0 and y <= 720: - img[719-y][1279-x] = c[1] - dist_array[y][1279-x] = \ - np.array([c[0], c[1], c[2]], dtype=np.float32) + img[719 - y][1279 - x] = c[1] + dist_array[y][1279 - x] = np.array( + [c[0], c[1], c[2]], dtype=np.float32 + ) # right depth image if focus == "Right": point = np.array([c[0], c[2], c[1], 1]) pixel = np.matmul(m, point) - x, y = int(pixel[0]/pixel[2]), int(pixel[1]/pixel[2]) + x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) if x >= 0 and x < 1280 and y >= 0 and y < 720: - img[y][1279-x] = -c[1] - dist_array[y][1279-x] = \ - np.array([c[0], c[1], c[2]], dtype=np.float32) + img[y][1279 - x] = -c[1] + dist_array[y][1279 - x] = np.array( + [c[0], c[1], c[2]], dtype=np.float32 + ) return dist_array -if __name__ == '__main__': +if __name__ == "__main__": lidar_distance = LidarDistance() lidar_distance.listener() diff --git a/code/perception/src/lidar_filter_utility.py b/code/perception/src/lidar_filter_utility.py index 4cd50260..b5904e4b 100755 --- a/code/perception/src/lidar_filter_utility.py +++ b/code/perception/src/lidar_filter_utility.py @@ -3,9 +3,16 @@ # https://gist.github.com/bigsnarfdude/bbfdf343cc2fc818dc08b58c0e1374ae -def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf, - max_y=np.inf, min_z=-np.inf, max_z=np.inf): - """ Compute a bounding_box filter on the given points +def bounding_box( + points, + min_x=-np.inf, + max_x=np.inf, + min_y=-np.inf, + max_y=np.inf, + min_z=-np.inf, + max_z=np.inf, +): + """Compute a bounding_box filter on the given points Parameters ---------- @@ -31,9 +38,9 @@ def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf, """ - bound_x = np.logical_and(points['x'] > min_x, points['x'] < max_x) - bound_y = np.logical_and(points['y'] > min_y, points['y'] < max_y) - bound_z = np.logical_and(points['z'] > min_z, points['z'] < max_z) + bound_x = np.logical_and(points["x"] > min_x, points["x"] < max_x) + bound_y = np.logical_and(points["y"] > min_y, points["y"] < max_y) + bound_z = np.logical_and(points["z"] > min_z, points["z"] < max_z) bb_filter = bound_x & bound_y & bound_z @@ -42,7 +49,7 @@ def bounding_box(points, min_x=-np.inf, max_x=np.inf, min_y=-np.inf, # https://stackoverflow.com/questions/15575878/how-do-you-remove-a-column-from-a-structured-numpy-array def remove_field_name(a, name): - """ Removes a column from a structured numpy array + """Removes a column from a structured numpy array :param a: structured numoy array :param name: name of the column to remove diff --git a/code/perception/src/position_heading_filter_debug_node.py b/code/perception/src/position_heading_filter_debug_node.py index 25d4c901..0c1428f2 100755 --- a/code/perception/src/position_heading_filter_debug_node.py +++ b/code/perception/src/position_heading_filter_debug_node.py @@ -10,6 +10,7 @@ from ros_compatibility.node import CompatibleNode from geometry_msgs.msg import PoseStamped from std_msgs.msg import Float32, Header + # from tf.transformations import euler_from_quaternion from std_msgs.msg import Float32MultiArray import rospy @@ -26,6 +27,7 @@ class position_heading_filter_debug_node(CompatibleNode): Node publishes a filtered gps signal. This is achieved using a rolling average. """ + def __init__(self): """ Constructor / Setup @@ -33,15 +35,16 @@ def __init__(self): """ super(position_heading_filter_debug_node, self).__init__( - 'position_heading_filter_debug_node') + "position_heading_filter_debug_node" + ) # basic info self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", "0.05") # carla attributes - CARLA_HOST = os.environ.get('CARLA_HOST', 'paf-carla-simulator-1') - CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + CARLA_HOST = os.environ.get("CARLA_HOST", "paf-carla-simulator-1") + CARLA_PORT = int(os.environ.get("CARLA_PORT", "2000")) self.client = carla.Client(CARLA_HOST, CARLA_PORT) self.world = None self.carla_car = None @@ -66,11 +69,11 @@ def __init__(self): # csv file attributes/ flags for plots self.csv_x_created = False - self.csv_file_path_x = '' + self.csv_file_path_x = "" self.csv_y_created = False - self.csv_file_path_y = '' + self.csv_file_path_y = "" self.csv_heading_created = False - self.csv_file_path_heading = '' + self.csv_file_path_heading = "" self.loginfo("Position Heading Filter Debug node started") @@ -81,40 +84,46 @@ def __init__(self): PoseStamped, f"/paf/{self.role_name}/current_pos", self.set_current_pos, - qos_profile=1) + qos_profile=1, + ) # Current_heading subscriber: self.current_heading_subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/current_heading", self.set_current_heading, - qos_profile=1) + qos_profile=1, + ) # test_filter_pos subscriber: self.test_filter_pos_subscriber = self.new_subscription( PoseStamped, f"/paf/{self.role_name}/kalman_pos", self.set_test_filter_pos, - qos_profile=1) + qos_profile=1, + ) # test_filter_heading subscriber: self.test_filter_heading_subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/kalman_heading", self.set_test_filter_heading, - qos_profile=1) + qos_profile=1, + ) # Unfiltered_pos subscriber: self.unfiltered_pos_subscriber = self.new_subscription( PoseStamped, f"/paf/{self.role_name}/unfiltered_pos", self.set_unfiltered_pos, - qos_profile=1) + qos_profile=1, + ) # Unfiltered_heading subscriber: self.unfiltered_heading_subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/unfiltered_heading", self.set_unfiltered_heading, - qos_profile=1) + qos_profile=1, + ) # endregion Subscriber END @@ -122,24 +131,20 @@ def __init__(self): # ideal carla publisher for easier debug with rqt_plot self.carla_heading_publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/carla_current_heading", - qos_profile=1) + Float32, f"/paf/{self.role_name}/carla_current_heading", qos_profile=1 + ) self.carla_pos_publisher = self.new_publisher( - PoseStamped, - f"/paf/{self.role_name}/carla_current_pos", - qos_profile=1) + PoseStamped, f"/paf/{self.role_name}/carla_current_pos", qos_profile=1 + ) # Error Publisher self.position_debug_publisher = self.new_publisher( - Float32MultiArray, - f"/paf/{self.role_name}/position_debug", - qos_profile=1) + Float32MultiArray, f"/paf/{self.role_name}/position_debug", qos_profile=1 + ) self.heading_debug_publisher = self.new_publisher( - Float32MultiArray, - f"/paf/{self.role_name}/heading_debug", - qos_profile=1) + Float32MultiArray, f"/paf/{self.role_name}/heading_debug", qos_profile=1 + ) # endregion Publisher END @@ -198,10 +203,9 @@ def save_position_data(self): return # Specify the path to the folder where you want to save the data - base_path = ('/workspace/code/perception/' - 'src/experiments/' + FOLDER_PATH) - folder_path_x = base_path + '/x_error' - folder_path_y = base_path + '/y_error' + base_path = "/workspace/code/perception/" "src/experiments/" + FOLDER_PATH + folder_path_x = base_path + "/x_error" + folder_path_y = base_path + "/y_error" # Ensure the directories exist os.makedirs(folder_path_x, exist_ok=True) os.makedirs(folder_path_y, exist_ok=True) @@ -231,9 +235,8 @@ def save_heading_data(self): return # Specify the path to the folder where you want to save the data - base_path = ('/workspace/code/perception/' - 'src/experiments' + FOLDER_PATH) - folder_path_heading = base_path + '/heading_error' + base_path = "/workspace/code/perception/" "src/experiments" + FOLDER_PATH + folder_path_heading = base_path + "/heading_error" # Ensure the directories exist os.makedirs(folder_path_heading, exist_ok=True) @@ -248,81 +251,93 @@ def save_heading_data(self): # helper methods for writing into csv files def write_csv_heading(self): - with open(self.csv_file_path_heading, 'a', newline='') as file: + with open(self.csv_file_path_heading, "a", newline="") as file: writer = csv.writer(file) # Check if file is empty if os.stat(self.csv_file_path_heading).st_size == 0: - writer.writerow([ - "Time", - "Unfiltered", - "Ideal (Carla)", - "Current", - "Test Filter", - "Unfiltered Error", - "Current Error" - "Test Filter Error", - ]) - writer.writerow([rospy.get_time(), - self.unfiltered_heading.data, - self.carla_current_heading, - self.current_heading.data, - self.test_filter_heading.data, - self.heading_debug_data.data[0], - self.heading_debug_data.data[1], - self.heading_debug_data.data[2] - ]) + writer.writerow( + [ + "Time", + "Unfiltered", + "Ideal (Carla)", + "Current", + "Test Filter", + "Unfiltered Error", + "Current Error" "Test Filter Error", + ] + ) + writer.writerow( + [ + rospy.get_time(), + self.unfiltered_heading.data, + self.carla_current_heading, + self.current_heading.data, + self.test_filter_heading.data, + self.heading_debug_data.data[0], + self.heading_debug_data.data[1], + self.heading_debug_data.data[2], + ] + ) def write_csv_x(self): - with open(self.csv_file_path_x, 'a', newline='') as file: + with open(self.csv_file_path_x, "a", newline="") as file: writer = csv.writer(file) # Check if file is empty and add first row if os.stat(self.csv_file_path_x).st_size == 0: - writer.writerow([ - "Time", - "Unfiltered", - "Ideal (Carla)", - "Current", - "Test Filter", - "Unfiltered Error", - "Current Error", - "Test Filter Error" - ]) - writer.writerow([ - rospy.get_time(), - self.unfiltered_pos.pose.position.x, - self.carla_current_pos.x, - self.current_pos.pose.position.x, - self.test_filter_pos.pose.position.x, - self.position_debug_data.data[8], - self.position_debug_data.data[11], - self.position_debug_data.data[14] - ]) + writer.writerow( + [ + "Time", + "Unfiltered", + "Ideal (Carla)", + "Current", + "Test Filter", + "Unfiltered Error", + "Current Error", + "Test Filter Error", + ] + ) + writer.writerow( + [ + rospy.get_time(), + self.unfiltered_pos.pose.position.x, + self.carla_current_pos.x, + self.current_pos.pose.position.x, + self.test_filter_pos.pose.position.x, + self.position_debug_data.data[8], + self.position_debug_data.data[11], + self.position_debug_data.data[14], + ] + ) def write_csv_y(self): - with open(self.csv_file_path_y, 'a', newline='') as file: + with open(self.csv_file_path_y, "a", newline="") as file: writer = csv.writer(file) # Check if file is empty and add first row if os.stat(self.csv_file_path_y).st_size == 0: - writer.writerow([ - "Time", - "Unfiltered", - "Ideal (Carla)", - "Current", - "Test Filter", - "Unfiltered Error", - "Current Error", - "Test Filter Error" - ]) - writer.writerow([ - rospy.get_time(), - self.unfiltered_pos.pose.position.y, - self.carla_current_pos.y, - self.current_pos.pose.position.y, - self.test_filter_pos.pose.position.y, - self.position_debug_data.data[9], - self.position_debug_data.data[12], - self.position_debug_data.data[15] - ]) + writer.writerow( + [ + "Time", + "Unfiltered", + "Ideal (Carla)", + "Current", + "Test Filter", + "Unfiltered Error", + "Current Error", + "Test Filter Error", + ] + ) + writer.writerow( + [ + rospy.get_time(), + self.unfiltered_pos.pose.position.y, + self.carla_current_pos.y, + self.current_pos.pose.position.y, + self.test_filter_pos.pose.position.y, + self.position_debug_data.data[9], + self.position_debug_data.data[12], + self.position_debug_data.data[15], + ] + ) # endregion CSV data save methods @@ -331,7 +346,7 @@ def set_carla_attributes(self): This method sets the carla attributes. """ for actor in self.world.get_actors(): - if actor.attributes.get('role_name') == "hero": + if actor.attributes.get("role_name") == "hero": self.carla_car = actor break if self.carla_car is None: @@ -349,9 +364,8 @@ def set_carla_attributes(self): # -> convert to radians # -> also flip the sign to minus self.carla_current_heading = -math.radians( - self.carla_car.get_transform() - .rotation.yaw - ) + self.carla_car.get_transform().rotation.yaw + ) def position_debug(self): """ @@ -418,42 +432,37 @@ def position_debug(self): debug.data[7] = self.test_filter_pos.pose.position.y # error between carla_current_pos and unfiltered_pos - debug.data[8] = (self.carla_current_pos.x - - self.unfiltered_pos.pose.position.x) - debug.data[9] = (self.carla_current_pos.y - - self.unfiltered_pos.pose.position.y) - debug.data[10] = math.sqrt((self.carla_current_pos.x - - self.unfiltered_pos.pose.position.x)**2 - + (self.carla_current_pos.y - - self.unfiltered_pos.pose.position.y)**2) + debug.data[8] = self.carla_current_pos.x - self.unfiltered_pos.pose.position.x + debug.data[9] = self.carla_current_pos.y - self.unfiltered_pos.pose.position.y + debug.data[10] = math.sqrt( + (self.carla_current_pos.x - self.unfiltered_pos.pose.position.x) ** 2 + + (self.carla_current_pos.y - self.unfiltered_pos.pose.position.y) ** 2 + ) # error between carla_current_pos and current_pos - debug.data[11] = (self.carla_current_pos.x - - self.current_pos.pose.position.x) - debug.data[12] = (self.carla_current_pos.y - - self.current_pos.pose.position.y) - debug.data[13] = math.sqrt((self.carla_current_pos.x - - self.current_pos.pose.position.x)**2 - + (self.carla_current_pos.y - - self.current_pos.pose.position.y)**2) + debug.data[11] = self.carla_current_pos.x - self.current_pos.pose.position.x + debug.data[12] = self.carla_current_pos.y - self.current_pos.pose.position.y + debug.data[13] = math.sqrt( + (self.carla_current_pos.x - self.current_pos.pose.position.x) ** 2 + + (self.carla_current_pos.y - self.current_pos.pose.position.y) ** 2 + ) # error between carla_current_pos and test_filter_pos - debug.data[14] = (self.carla_current_pos.x - - self.test_filter_pos.pose.position.x) - debug.data[15] = (self.carla_current_pos.y - - self.test_filter_pos.pose.position.y) - debug.data[16] = math.sqrt((self.carla_current_pos.x - - self.test_filter_pos.pose.position.x)**2 - + (self.carla_current_pos.y - - self.test_filter_pos.pose.position.y)**2) + debug.data[14] = self.carla_current_pos.x - self.test_filter_pos.pose.position.x + debug.data[15] = self.carla_current_pos.y - self.test_filter_pos.pose.position.y + debug.data[16] = math.sqrt( + (self.carla_current_pos.x - self.test_filter_pos.pose.position.x) ** 2 + + (self.carla_current_pos.y - self.test_filter_pos.pose.position.y) ** 2 + ) self.position_debug_data = debug self.position_debug_publisher.publish(debug) # for easier debugging with rqt_plot # Publish carla Location as PoseStamped: - self.carla_pos_publisher.publish(carla_location_to_pose_stamped( - self.carla_current_pos)) + self.carla_pos_publisher.publish( + carla_location_to_pose_stamped(self.carla_current_pos) + ) def heading_debug(self): """ @@ -495,16 +504,13 @@ def heading_debug(self): debug.data[3] = self.test_filter_heading.data # error between carla_current_heading and unfiltered_heading - debug.data[4] = (self.carla_current_heading - - self.unfiltered_heading.data) + debug.data[4] = self.carla_current_heading - self.unfiltered_heading.data # error between carla_current_heading and current_heading - debug.data[5] = (self.carla_current_heading - - self.current_heading.data) + debug.data[5] = self.carla_current_heading - self.current_heading.data # error between carla_current_heading and test_filter_heading - debug.data[6] = (self.carla_current_heading - - self.test_filter_heading.data) + debug.data[6] = self.carla_current_heading - self.test_filter_heading.data self.heading_debug_data = debug self.heading_debug_publisher.publish(debug) @@ -566,16 +572,16 @@ def loop(): def create_file(folder_path): - ''' + """ This function creates a new csv file in the folder_path in correct sequence looking like data_00.csv, data_01.csv, ... and returns the path to the file. - ''' + """ i = 0 while True: - file_path = f'{folder_path}/data_{str(i).zfill(2)}.csv' + file_path = f"{folder_path}/data_{str(i).zfill(2)}.csv" if not os.path.exists(file_path): - with open(file_path, 'w', newline=''): + with open(file_path, "w", newline=""): pass return file_path i += 1 diff --git a/code/perception/src/position_heading_publisher_node.py b/code/perception/src/position_heading_publisher_node.py index f5b62e72..c54e7d23 100755 --- a/code/perception/src/position_heading_publisher_node.py +++ b/code/perception/src/position_heading_publisher_node.py @@ -7,6 +7,7 @@ from ros_compatibility.node import CompatibleNode from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import NavSatFix, Imu + # from nav_msgs.msg import Odometry from std_msgs.msg import Float32, String from coordinate_transformation import CoordinateTransformer @@ -46,7 +47,8 @@ def __init__(self): """ super(PositionHeadingPublisherNode, self).__init__( - 'position_heading_publisher_node') + "position_heading_publisher_node" + ) """ Possible Filters: @@ -56,9 +58,12 @@ def __init__(self): # Filter used: self.pos_filter = self.get_param("pos_filter", "Kalman") self.heading_filter = self.get_param("heading_filter", "Kalman") - self.loginfo("position_heading_publisher_node started with Pos Filter:" - + self.pos_filter + - " and Heading Filter: " + self.heading_filter) + self.loginfo( + "position_heading_publisher_node started with Pos Filter:" + + self.pos_filter + + " and Heading Filter: " + + self.heading_filter + ) # basic info self.role_name = self.get_param("role_name", "hero") @@ -71,25 +76,28 @@ def __init__(self): self.avg_xyz = np.zeros((GPS_RUNNING_AVG_ARGS, 3)) self.avg_gps_counter: int = 0 - # region Subscriber START + # region Subscriber START self.map_sub = self.new_subscription( String, "/carla/" + self.role_name + "/OpenDRIVE", self.get_geoRef, - qos_profile=1) + qos_profile=1, + ) self.imu_subscriber = self.new_subscription( Imu, "/carla/" + self.role_name + "/IMU", self.publish_unfiltered_heading, - qos_profile=1) + qos_profile=1, + ) self.gps_subscriber = self.new_subscription( NavSatFix, "/carla/" + self.role_name + "/GPS", self.publish_unfiltered_gps, - qos_profile=1) + qos_profile=1, + ) # Create subscribers depending on the filter used # Pos Filter: @@ -98,13 +106,15 @@ def __init__(self): PoseStamped, "/paf/" + self.role_name + "/kalman_pos", self.publish_kalman_pos_as_current_pos, - qos_profile=1) + qos_profile=1, + ) elif self.pos_filter == "RunningAvg": self.gps_subscriber_for_running_avg = self.new_subscription( NavSatFix, "/carla/" + self.role_name + "/GPS", self.publish_running_avg_pos_as_current_pos, - qos_profile=1) + qos_profile=1, + ) elif self.pos_filter == "None": # No additional subscriber needed since the unfiltered GPS data is # subscribed by self.gps_subscriber @@ -118,7 +128,8 @@ def __init__(self): Float32, "/paf/" + self.role_name + "/kalman_heading", self.publish_current_heading, - qos_profile=1) + qos_profile=1, + ) elif self.heading_filter == "None": # No additional subscriber needed since the unfiltered heading # data is subscribed by self.imu_subscriber @@ -126,35 +137,31 @@ def __init__(self): # insert additional elifs for other filters here - # endregion Subscriber END + # endregion Subscriber END - # region Publisher START + # region Publisher START # Orientation self.unfiltered_heading_publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/unfiltered_heading", - qos_profile=1) + Float32, f"/paf/{self.role_name}/unfiltered_heading", qos_profile=1 + ) # 3D Odometry (GPS) for Filters self.unfiltered_gps_publisher = self.new_publisher( - PoseStamped, - f"/paf/{self.role_name}/unfiltered_pos", - qos_profile=1) + PoseStamped, f"/paf/{self.role_name}/unfiltered_pos", qos_profile=1 + ) # Publishes current_pos depending on the filter used self.cur_pos_publisher = self.new_publisher( - PoseStamped, - f"/paf/{self.role_name}/current_pos", - qos_profile=1) + PoseStamped, f"/paf/{self.role_name}/current_pos", qos_profile=1 + ) self.__heading: float = 0 self.__heading_publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/current_heading", - qos_profile=1) + Float32, f"/paf/{self.role_name}/current_heading", qos_profile=1 + ) # endregion Publisher END -# region HEADING FUNCTIONS + # region HEADING FUNCTIONS def publish_unfiltered_heading(self, data: Imu): """ This method is called when new IMU data is received. @@ -162,10 +169,12 @@ def publish_unfiltered_heading(self, data: Imu): :param data: new IMU measurement :return: """ - data_orientation_q = [data.orientation.x, - data.orientation.y, - data.orientation.z, - data.orientation.w] + data_orientation_q = [ + data.orientation.x, + data.orientation.y, + data.orientation.z, + data.orientation.w, + ] heading = quat_to_heading(data_orientation_q) @@ -205,9 +214,9 @@ def publish_current_heading(self, data: Float32): # insert new heading functions here... -# endregion HEADING FUNCTIONS END + # endregion HEADING FUNCTIONS END -# region POSITION FUNCTIONS + # region POSITION FUNCTIONS def publish_running_avg_pos_as_current_pos(self, data: NavSatFix): """ @@ -314,7 +323,7 @@ def publish_unfiltered_gps(self, data: NavSatFix): # insert new position functions here... -# endregion POSITION FUNCTIONS END + # endregion POSITION FUNCTIONS END def get_geoRef(self, opendrive: String): """_summary_ @@ -336,8 +345,8 @@ def get_geoRef(self, opendrive: String): indexLatEnd = geoRefText.find(" ", indexLat) indexLonEnd = geoRefText.find(" ", indexLon) - latValue = float(geoRefText[indexLat + len(latString):indexLatEnd]) - lonValue = float(geoRefText[indexLon + len(lonString):indexLonEnd]) + latValue = float(geoRefText[indexLat + len(latString) : indexLatEnd]) + lonValue = float(geoRefText[indexLon + len(lonString) : indexLonEnd]) CoordinateTransformer.la_ref = latValue CoordinateTransformer.ln_ref = lonValue diff --git a/code/perception/src/traffic_light_detection/src/data_generation/weights_organizer.py b/code/perception/src/traffic_light_detection/src/data_generation/weights_organizer.py index 5e4d9d29..01b64189 100644 --- a/code/perception/src/traffic_light_detection/src/data_generation/weights_organizer.py +++ b/code/perception/src/traffic_light_detection/src/data_generation/weights_organizer.py @@ -18,9 +18,11 @@ def __init__(self, cfg, model): try: os.makedirs(self.cfg.WEIGHTS_PATH, exist_ok=True) except FileExistsError: - sys.exit(f"The directory {self.cfg.WEIGHTS_PATH} already exists." - f"Cannot create weights-directory for training." - f"Try again in at least one minute.") + sys.exit( + f"The directory {self.cfg.WEIGHTS_PATH} already exists." + f"Cannot create weights-directory for training." + f"Try again in at least one minute." + ) def save(self, accuracy, val_accuracy): """ @@ -28,14 +30,18 @@ def save(self, accuracy, val_accuracy): @param accuracy: Accuracy of the model in the last epoch @param val_accuracy: Accuracy of the model on the validation-subset """ - filename = self.cfg.WEIGHTS_PATH + f"model_acc_{round(accuracy, 2)}" \ - + f"_val_{round(val_accuracy, 2)}.pt" + filename = ( + self.cfg.WEIGHTS_PATH + + f"model_acc_{round(accuracy, 2)}" + + f"_val_{round(val_accuracy, 2)}.pt" + ) if len(self.best) == 0: torch.save(self.model.state_dict(), filename) self.best.append((accuracy, val_accuracy, filename)) - elif val_accuracy > self.best[len(self.best) - 1][1] or \ - (val_accuracy >= self.best[len(self.best) - 1][1] and - accuracy > self.best[len(self.best) - 1][0]): + elif val_accuracy > self.best[len(self.best) - 1][1] or ( + val_accuracy >= self.best[len(self.best) - 1][1] + and accuracy > self.best[len(self.best) - 1][0] + ): if len(self.best) == 1: delete = self.best[0][2] diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_config.py b/code/perception/src/traffic_light_detection/src/traffic_light_config.py index e1c720c2..39c63da1 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_config.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_config.py @@ -7,7 +7,7 @@ class TrafficLightConfig: def __init__(self): # General settings - self.DEVICE = ('cuda' if torch.cuda.is_available() else 'cpu') + self.DEVICE = "cuda" if torch.cuda.is_available() else "cpu" self.TIME = datetime.now().strftime("%d.%m.%Y_%H.%M") # Training diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py index f44d2c31..174c6519 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/classification_model.py @@ -12,17 +12,21 @@ def __init__(self, num_classes, in_channels=3): @param num_classes: Number of classes """ super(ClassificationModel, self).__init__() - self.conv1 = nn.Conv2d(in_channels=in_channels, out_channels=4, - kernel_size=5, padding='same') + self.conv1 = nn.Conv2d( + in_channels=in_channels, out_channels=4, kernel_size=5, padding="same" + ) self.batch_norm1 = nn.BatchNorm2d(num_features=4) - self.conv2 = nn.Conv2d(in_channels=4, out_channels=4, kernel_size=5, - padding='same') + self.conv2 = nn.Conv2d( + in_channels=4, out_channels=4, kernel_size=5, padding="same" + ) self.max_pool1 = nn.MaxPool2d(kernel_size=(2, 2)) - self.conv3 = nn.Conv2d(in_channels=4, out_channels=4, kernel_size=3, - padding='same') + self.conv3 = nn.Conv2d( + in_channels=4, out_channels=4, kernel_size=3, padding="same" + ) self.max_pool2 = nn.MaxPool2d(kernel_size=(2, 2)) - self.conv4 = nn.Conv2d(in_channels=4, out_channels=4, kernel_size=3, - padding='same') + self.conv4 = nn.Conv2d( + in_channels=4, out_channels=4, kernel_size=3, padding="same" + ) self.max_pool3 = nn.MaxPool2d(kernel_size=(2, 2)) self.flatten = nn.Flatten() self.dropout = nn.Dropout(p=0.3) @@ -63,6 +67,8 @@ def load_model(cfg): print(f"Pretrained model loaded from {path}") return model except Exception as e: - print(f"No pretrained model found at {path}: {e}\n" - f"Created new model with random weights.") + print( + f"No pretrained model found at {path}: {e}\n" + f"Created new model with random weights." + ) return model.eval() diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py index cb3d9e5c..d9922795 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_inference.py @@ -2,10 +2,14 @@ import torch.cuda import torchvision.transforms as t -from traffic_light_detection.src.traffic_light_detection.transforms \ - import Normalize, ResizeAndPadToSquare, load_image -from traffic_light_detection.src.traffic_light_detection.classification_model \ - import ClassificationModel +from traffic_light_detection.src.traffic_light_detection.transforms import ( + Normalize, + ResizeAndPadToSquare, + load_image, +) +from traffic_light_detection.src.traffic_light_detection.classification_model import ( + ClassificationModel, +) from torchvision.transforms import ToTensor from traffic_light_detection.src.traffic_light_config import TrafficLightConfig @@ -15,18 +19,19 @@ def parse_args(): Parses arguments for execution given by the command line. @return: Parsed arguments """ - parser = argparse.ArgumentParser(description='Inference traffic light ' - 'detection') - parser.add_argument('--model', - default='/opt/project/code/perception/src/' - 'traffic_light_detection/models/' - '05.12.2022_17.47/' - 'model_acc_99.53_val_100.0.pt', - help='path to pretrained model', - type=str) - parser.add_argument('--image', default=None, - help='/dataset/val/green/green_83.png', - type=str) + parser = argparse.ArgumentParser(description="Inference traffic light " "detection") + parser.add_argument( + "--model", + default="/opt/project/code/perception/src/" + "traffic_light_detection/models/" + "05.12.2022_17.47/" + "model_acc_99.53_val_100.0.pt", + help="path to pretrained model", + type=str, + ) + parser.add_argument( + "--image", default=None, help="/dataset/val/green/green_83.png", type=str + ) return parser.parse_args() @@ -39,19 +44,17 @@ def __init__(self, model_path): """ self.cfg = TrafficLightConfig() self.cfg.MODEL_PATH = model_path - self.transforms = t.Compose([ - ToTensor(), - ResizeAndPadToSquare([32, 32]), - Normalize(mean=[0.5, 0.5, 0.5], std=[0.5, 0.5, 0.5]) - ]) + self.transforms = t.Compose( + [ + ToTensor(), + ResizeAndPadToSquare([32, 32]), + Normalize(mean=[0.5, 0.5, 0.5], std=[0.5, 0.5, 0.5]), + ] + ) self.model = ClassificationModel.load_model(self.cfg) self.model = self.model.to(self.cfg.DEVICE) - self.class_dict = {0: 'Backside', - 1: 'Green', - 2: 'Red', - 3: 'Side', - 4: 'Yellow'} + self.class_dict = {0: "Backside", 1: "Green", 2: "Red", 3: "Side", 4: "Yellow"} def __call__(self, img): """ @@ -68,7 +71,7 @@ def __call__(self, img): # main function for testing purposes -if __name__ == '__main__': +if __name__ == "__main__": args = parse_args() image_path = args.image image = load_image(image_path) diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py index 1322d024..dcf4fe7b 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/traffic_light_training.py @@ -10,12 +10,17 @@ from ruamel.yaml import YAML import sys import os -sys.path.append(os.path.abspath(sys.path[0] + '/..')) -from traffic_light_detection.transforms import Normalize, \ - ResizeAndPadToSquare, load_image # noqa: E402 + +sys.path.append(os.path.abspath(sys.path[0] + "/..")) +from traffic_light_detection.transforms import ( + Normalize, + ResizeAndPadToSquare, + load_image, +) # noqa: E402 from data_generation.weights_organizer import WeightsOrganizer # noqa: E402 -from traffic_light_detection.classification_model import ClassificationModel \ - # noqa: E402 +from traffic_light_detection.classification_model import ( + ClassificationModel, +) # noqa: E402 from traffic_light_config import TrafficLightConfig # noqa: E402 @@ -27,38 +32,50 @@ def __init__(self, cfg): @param cfg: Config file for traffic light classification """ self.cfg = cfg - train_transforms = t.Compose([ - ToTensor(), - ResizeAndPadToSquare([32, 32]), - Normalize(mean=[0.5, 0.5, 0.5], std=[0.5, 0.5, 0.5]) - # ApplyMask(dataset_root + "/mask.png") - ]) - val_transforms = t.Compose([ - ToTensor(), - ResizeAndPadToSquare([32, 32]), - Normalize(mean=[0.5, 0.5, 0.5], std=[0.5, 0.5, 0.5]) - ]) - self.train_dataset = ImageFolder(root=self.cfg.DATASET_PATH + "/train", - transform=train_transforms, - loader=load_image) - self.train_loader = DataLoader(dataset=self.train_dataset, - batch_size=self.cfg.BATCH_SIZE, - num_workers=self.cfg.NUM_WORKERS, - shuffle=True) - self.val_dataset = ImageFolder(root=self.cfg.DATASET_PATH + "/val", - transform=val_transforms, - loader=load_image) - self.val_loader = DataLoader(dataset=self.val_dataset, - batch_size=self.cfg.BATCH_SIZE, - num_workers=self.cfg.NUM_WORKERS) - self.model = ClassificationModel(num_classes=self.cfg.NUM_CLASSES, - in_channels=self.cfg.NUM_CHANNELS) + train_transforms = t.Compose( + [ + ToTensor(), + ResizeAndPadToSquare([32, 32]), + Normalize(mean=[0.5, 0.5, 0.5], std=[0.5, 0.5, 0.5]), + # ApplyMask(dataset_root + "/mask.png") + ] + ) + val_transforms = t.Compose( + [ + ToTensor(), + ResizeAndPadToSquare([32, 32]), + Normalize(mean=[0.5, 0.5, 0.5], std=[0.5, 0.5, 0.5]), + ] + ) + self.train_dataset = ImageFolder( + root=self.cfg.DATASET_PATH + "/train", + transform=train_transforms, + loader=load_image, + ) + self.train_loader = DataLoader( + dataset=self.train_dataset, + batch_size=self.cfg.BATCH_SIZE, + num_workers=self.cfg.NUM_WORKERS, + shuffle=True, + ) + self.val_dataset = ImageFolder( + root=self.cfg.DATASET_PATH + "/val", + transform=val_transforms, + loader=load_image, + ) + self.val_loader = DataLoader( + dataset=self.val_dataset, + batch_size=self.cfg.BATCH_SIZE, + num_workers=self.cfg.NUM_WORKERS, + ) + self.model = ClassificationModel( + num_classes=self.cfg.NUM_CLASSES, in_channels=self.cfg.NUM_CHANNELS + ) self.model = self.model.to(self.cfg.DEVICE) self.optimizer = Adam(self.model.parameters()) self.lr_scheduler = ExponentialLR(self.optimizer, 0.95) self.loss_function = torch.nn.CrossEntropyLoss() - self.weights_organizer = WeightsOrganizer(cfg=self.cfg, - model=self.model) + self.weights_organizer = WeightsOrganizer(cfg=self.cfg, model=self.model) self.live = Live() @@ -76,8 +93,12 @@ def run(self): self.live.log_metric("validation/loss", loss) self.lr_scheduler.step() self.live.next_step() - tepoch.set_postfix(loss=epoch_loss, accuracy=epoch_correct, - val_loss=loss, val_accuracy=correct) + tepoch.set_postfix( + loss=epoch_loss, + accuracy=epoch_correct, + val_loss=loss, + val_accuracy=correct, + ) tepoch.update(1) print(tepoch) self.weights_organizer.save(epoch_correct, correct) @@ -99,14 +120,14 @@ def epoch(self): loss = self.loss_function(outputs, labels) epoch_loss += loss.item() _, predictions = torch.max(outputs.data, 1) - corr = (predictions == labels) + corr = predictions == labels epoch_correct += corr.sum().item() loss.backward() self.optimizer.step() epoch_loss /= len(self.train_dataset) epoch_correct /= len(self.train_dataset) - return epoch_loss, 100. * epoch_correct + return epoch_loss, 100.0 * epoch_correct def validate(self): """ @@ -114,7 +135,7 @@ def validate(self): @return: Average loss and accuracy of the net on the validation-subset """ self.model.eval() - val_loss = 0. + val_loss = 0.0 val_correct = 0 for i, data in enumerate(self.val_loader): images = data[0].to(self.cfg.DEVICE) @@ -125,22 +146,22 @@ def validate(self): loss = self.loss_function(outputs, labels) val_loss += loss.item() _, predictions = torch.max(outputs.data, 1) - corr = (predictions == labels) + corr = predictions == labels val_correct += corr.sum().item() val_loss /= len(self.val_dataset) val_correct /= len(self.val_dataset) - return val_loss, 100. * val_correct + return val_loss, 100.0 * val_correct -if __name__ == '__main__': +if __name__ == "__main__": yaml = YAML(typ="safe") with open("params.yaml") as f: params = yaml.load(f) cfg = TrafficLightConfig() - cfg.EPOCHS = params['train']['epochs'] - cfg.BATCH_SIZE = params['train']['batch_size'] + cfg.EPOCHS = params["train"]["epochs"] + cfg.BATCH_SIZE = params["train"]["batch_size"] print(f"Computation device: {cfg.DEVICE}\n") tr = TrafficLightTraining(cfg) tr.run() diff --git a/code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py b/code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py index 41dc8bfc..82aba2bc 100644 --- a/code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py +++ b/code/perception/src/traffic_light_detection/src/traffic_light_detection/transforms.py @@ -8,7 +8,7 @@ def load_image(path): Loads an image from the given path @rtype: RGB-coded PIL image """ - image = Image.open(path).convert('RGB') + image = Image.open(path).convert("RGB") return image @@ -56,7 +56,7 @@ def __init__(self, path): mask @param path: Path to the mask """ - self.mask = functional.to_tensor(Image.open(path).convert('L')) + self.mask = functional.to_tensor(Image.open(path).convert("L")) def __call__(self, image): mask = torchvision.transforms.Resize(image.shape[1:])(self.mask) diff --git a/code/perception/src/traffic_light_node.py b/code/perception/src/traffic_light_node.py index d7eee4b3..e8216ac1 100755 --- a/code/perception/src/traffic_light_node.py +++ b/code/perception/src/traffic_light_node.py @@ -10,8 +10,9 @@ from perception.msg import TrafficLightState from std_msgs.msg import Int16 from cv_bridge import CvBridge -from traffic_light_detection.src.traffic_light_detection.traffic_light_inference \ - import TrafficLightInference # noqa: E501 +from traffic_light_detection.src.traffic_light_detection.traffic_light_inference import ( + TrafficLightInference, +) # noqa: E501 import cv2 import numpy as np @@ -37,20 +38,19 @@ def setup_camera_subscriptions(self): msg_type=numpy_msg(ImageMsg), callback=self.handle_camera_image, topic=f"/paf/{self.role_name}/{self.side}/segmented_traffic_light", - qos_profile=1 + qos_profile=1, ) def setup_traffic_light_publishers(self): self.traffic_light_publisher = self.new_publisher( msg_type=TrafficLightState, topic=f"/paf/{self.role_name}/{self.side}/traffic_light_state", - qos_profile=1 + qos_profile=1, ) self.traffic_light_distance_publisher = self.new_publisher( msg_type=Int16, - topic=f"/paf/{self.role_name}/{self.side}" + - "/traffic_light_y_distance", - qos_profile=1 + topic=f"/paf/{self.role_name}/{self.side}" + "/traffic_light_y_distance", + qos_profile=1, ) def auto_invalidate_state(self): @@ -74,8 +74,12 @@ def handle_camera_image(self, image): rgb_image = cv2.cvtColor(cv2_image, cv2.COLOR_BGR2RGB) result, data = self.classifier(cv2_image) - if data[0][0] > 1e-15 and data[0][3] > 1e-15 or \ - data[0][0] > 1e-10 or data[0][3] > 1e-10: + if ( + data[0][0] > 1e-15 + and data[0][3] > 1e-15 + or data[0][0] > 1e-10 + or data[0][3] > 1e-10 + ): return # too uncertain, may not be a traffic light if not is_front(rgb_image): @@ -123,8 +127,7 @@ def is_front(image): mask = get_light_mask(image) # Find contours in the thresholded image, use only the largest one - contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, - cv2.CHAIN_APPROX_SIMPLE) + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) contours = sorted(contours, key=cv2.contourArea, reverse=True)[:1] contour = contours[0] if contours else None diff --git a/code/perception/src/vision_node.py b/code/perception/src/vision_node.py index 66bb4ad9..ef8158aa 100755 --- a/code/perception/src/vision_node.py +++ b/code/perception/src/vision_node.py @@ -3,13 +3,16 @@ from ros_compatibility.node import CompatibleNode import ros_compatibility as roscomp import torch -from torchvision.models.segmentation import DeepLabV3_ResNet101_Weights, \ - deeplabv3_resnet101 -from torchvision.models.detection.faster_rcnn import \ - FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, \ - FasterRCNN_ResNet50_FPN_V2_Weights, \ - fasterrcnn_resnet50_fpn_v2, \ - fasterrcnn_mobilenet_v3_large_320_fpn +from torchvision.models.segmentation import ( + DeepLabV3_ResNet101_Weights, + deeplabv3_resnet101, +) +from torchvision.models.detection.faster_rcnn import ( + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, + FasterRCNN_ResNet50_FPN_V2_Weights, + fasterrcnn_resnet50_fpn_v2, + fasterrcnn_mobilenet_v3_large_320_fpn, +) import torchvision.transforms as t import cv2 from rospy.numpy_msg import numpy_msg @@ -39,38 +42,41 @@ def __init__(self, name, **kwargs): # dictionary of pretrained models self.model_dict = { - "fasterrcnn_resnet50_fpn_v2": - (fasterrcnn_resnet50_fpn_v2( - weights=FasterRCNN_ResNet50_FPN_V2_Weights.DEFAULT), + "fasterrcnn_resnet50_fpn_v2": ( + fasterrcnn_resnet50_fpn_v2( + weights=FasterRCNN_ResNet50_FPN_V2_Weights.DEFAULT + ), FasterRCNN_ResNet50_FPN_V2_Weights.DEFAULT, "detection", - "pyTorch"), - "fasterrcnn_mobilenet_v3_large_320_fpn": - (fasterrcnn_mobilenet_v3_large_320_fpn( - weights=FasterRCNN_MobileNet_V3_Large_320_FPN_Weights.DEFAULT), + "pyTorch", + ), + "fasterrcnn_mobilenet_v3_large_320_fpn": ( + fasterrcnn_mobilenet_v3_large_320_fpn( + weights=FasterRCNN_MobileNet_V3_Large_320_FPN_Weights.DEFAULT + ), FasterRCNN_MobileNet_V3_Large_320_FPN_Weights.DEFAULT, "detection", - "pyTorch"), - "deeplabv3_resnet101": - (deeplabv3_resnet101( - weights=DeepLabV3_ResNet101_Weights.DEFAULT), + "pyTorch", + ), + "deeplabv3_resnet101": ( + deeplabv3_resnet101(weights=DeepLabV3_ResNet101_Weights.DEFAULT), DeepLabV3_ResNet101_Weights.DEFAULT, "segmentation", - "pyTorch"), - 'yolov8n': (YOLO, "yolov8n.pt", "detection", "ultralytics"), - 'yolov8s': (YOLO, "yolov8s.pt", "detection", "ultralytics"), - 'yolov8m': (YOLO, "yolov8m.pt", "detection", "ultralytics"), - 'yolov8l': (YOLO, "yolov8l.pt", "detection", "ultralytics"), - 'yolov8x': (YOLO, "yolov8x.pt", "detection", "ultralytics"), - 'yolo_nas_l': (NAS, "yolo_nas_l.pt", "detection", "ultralytics"), - 'yolo_nas_m': (NAS, "yolo_nas_m.pt", "detection", "ultralytics"), - 'yolo_nas_s': (NAS, "yolo_nas_s.pt", "detection", "ultralytics"), - 'rtdetr-l': (RTDETR, "rtdetr-l.pt", "detection", "ultralytics"), - 'rtdetr-x': (RTDETR, "rtdetr-x.pt", "detection", "ultralytics"), - 'yolov8x-seg': (YOLO, "yolov8x-seg.pt", "segmentation", - "ultralytics"), - 'sam_l': (SAM, "sam_l.pt", "detection", "ultralytics"), - 'FastSAM-x': (FastSAM, "FastSAM-x.pt", "detection", "ultralytics"), + "pyTorch", + ), + "yolov8n": (YOLO, "yolov8n.pt", "detection", "ultralytics"), + "yolov8s": (YOLO, "yolov8s.pt", "detection", "ultralytics"), + "yolov8m": (YOLO, "yolov8m.pt", "detection", "ultralytics"), + "yolov8l": (YOLO, "yolov8l.pt", "detection", "ultralytics"), + "yolov8x": (YOLO, "yolov8x.pt", "detection", "ultralytics"), + "yolo_nas_l": (NAS, "yolo_nas_l.pt", "detection", "ultralytics"), + "yolo_nas_m": (NAS, "yolo_nas_m.pt", "detection", "ultralytics"), + "yolo_nas_s": (NAS, "yolo_nas_s.pt", "detection", "ultralytics"), + "rtdetr-l": (RTDETR, "rtdetr-l.pt", "detection", "ultralytics"), + "rtdetr-x": (RTDETR, "rtdetr-x.pt", "detection", "ultralytics"), + "yolov8x-seg": (YOLO, "yolov8x-seg.pt", "segmentation", "ultralytics"), + "sam_l": (SAM, "sam_l.pt", "detection", "ultralytics"), + "FastSAM-x": (FastSAM, "FastSAM-x.pt", "detection", "ultralytics"), } # general setup @@ -82,8 +88,7 @@ def __init__(self, name, **kwargs): self.left = self.get_param("left") self.right = self.get_param("right") - self.device = torch.device("cuda" - if torch.cuda.is_available() else "cpu") + self.device = torch.device("cuda" if torch.cuda.is_available() else "cpu") self.depth_images = [] self.dist_arrays = None @@ -142,7 +147,7 @@ def setup_camera_subscriptions(self, side): msg_type=numpy_msg(ImageMsg), callback=self.handle_camera_image, topic=f"/carla/{self.role_name}/{side}/image", - qos_profile=1 + qos_profile=1, ) def setup_dist_array_subscription(self): @@ -154,8 +159,8 @@ def setup_dist_array_subscription(self): self.new_subscription( msg_type=numpy_msg(ImageMsg), callback=self.handle_dist_array, - topic='/paf/hero/Center/dist_array', - qos_profile=1 + topic="/paf/hero/Center/dist_array", + qos_profile=1, ) def setup_camera_publishers(self): @@ -170,25 +175,25 @@ def setup_camera_publishers(self): self.publisher_center = self.new_publisher( msg_type=numpy_msg(ImageMsg), topic=f"/paf/{self.role_name}/Center/segmented_image", - qos_profile=1 + qos_profile=1, ) if self.back: self.publisher_back = self.new_publisher( msg_type=numpy_msg(ImageMsg), topic=f"/paf/{self.role_name}/Back/segmented_image", - qos_profile=1 + qos_profile=1, ) if self.left: self.publisher_left = self.new_publisher( msg_type=numpy_msg(ImageMsg), topic=f"/paf/{self.role_name}/Left/segmented_image", - qos_profile=1 + qos_profile=1, ) if self.right: self.publisher_right = self.new_publisher( msg_type=numpy_msg(ImageMsg), topic=f"/paf/{self.role_name}/Right/segmented_image", - qos_profile=1 + qos_profile=1, ) def setup_object_distance_publishers(self): @@ -200,7 +205,8 @@ def setup_object_distance_publishers(self): self.distance_publisher = self.new_publisher( msg_type=Float32MultiArray, topic=f"/paf/{self.role_name}/{self.side}/object_distance", - qos_profile=1) + qos_profile=1, + ) def setup_traffic_light_publishers(self): """ @@ -210,7 +216,7 @@ def setup_traffic_light_publishers(self): self.traffic_light_publisher = self.new_publisher( msg_type=numpy_msg(ImageMsg), topic=f"/paf/{self.role_name}/{self.side}/segmented_traffic_light", - qos_profile=1 + qos_profile=1, ) def handle_camera_image(self, image): @@ -233,12 +239,11 @@ def handle_camera_image(self, image): vision_result = self.predict_ultralytics(image) # publish vision result to rviz - img_msg = self.bridge.cv2_to_imgmsg(vision_result, - encoding="rgb8") + img_msg = self.bridge.cv2_to_imgmsg(vision_result, encoding="rgb8") img_msg.header = image.header # publish img to corresponding angle topic - side = rospy.resolve_name(img_msg.header.frame_id).split('/')[2] + side = rospy.resolve_name(img_msg.header.frame_id).split("/")[2] if side == "Center": self.publisher_center.publish(img_msg) if side == "Back": @@ -259,9 +264,9 @@ def handle_dist_array(self, dist_array): # callback function for lidar depth image # since frequency is lower than image frequency # the latest lidar image is saved - dist_array = \ - self.bridge.imgmsg_to_cv2(img_msg=dist_array, - desired_encoding='passthrough') + dist_array = self.bridge.imgmsg_to_cv2( + img_msg=dist_array, desired_encoding="passthrough" + ) self.dist_arrays = dist_array def predict_torch(self, image): @@ -282,14 +287,16 @@ def predict_torch(self, image): self.model.eval() # preprocess image - cv_image = self.bridge.imgmsg_to_cv2(img_msg=image, - desired_encoding='passthrough') + cv_image = self.bridge.imgmsg_to_cv2( + img_msg=image, desired_encoding="passthrough" + ) cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) - preprocess = t.Compose([ - t.ToTensor(), - t.Normalize(mean=[0.485, 0.456, 0.406], - std=[0.229, 0.224, 0.225]) - ]) + preprocess = t.Compose( + [ + t.ToTensor(), + t.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]), + ] + ) input_image = preprocess(cv_image).unsqueeze(dim=0) # get prediction @@ -297,10 +304,10 @@ def predict_torch(self, image): prediction = self.model(input_image) # apply visualition - if (self.type == "detection"): + if self.type == "detection": vision_result = self.apply_bounding_boxes(cv_image, prediction[0]) - if (self.type == "segmentation"): - vision_result = self.create_mask(cv_image, prediction['out']) + if self.type == "segmentation": + vision_result = self.create_mask(cv_image, prediction["out"]) return vision_result @@ -321,8 +328,9 @@ def predict_ultralytics(self, image): """ # preprocess image - cv_image = self.bridge.imgmsg_to_cv2(img_msg=image, - desired_encoding='passthrough') + cv_image = self.bridge.imgmsg_to_cv2( + img_msg=image, desired_encoding="passthrough" + ) cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) # run model prediction @@ -347,9 +355,12 @@ def predict_ultralytics(self, image): # crop bounding box area out of depth image distances = np.asarray( - self.dist_arrays[int(pixels[1]):int(pixels[3]):1, - int(pixels[0]):int(pixels[2]):1, - ::]) + self.dist_arrays[ + int(pixels[1]) : int(pixels[3]) : 1, + int(pixels[0]) : int(pixels[2]) : 1, + ::, + ] + ) # set all 0 (black) values to np.inf (necessary if # you want to search for minimum) @@ -376,14 +387,14 @@ def predict_ultralytics(self, image): # copy actual lidar points obj_dist_min_x = self.min_x(dist_array=distances_copy) - obj_dist_min_abs_y = self.min_abs_y( - dist_array=distances_copy) + obj_dist_min_abs_y = self.min_abs_y(dist_array=distances_copy) # absolut distance to object for visualization abs_distance = np.sqrt( - obj_dist_min_x[0]**2 + - obj_dist_min_x[1]**2 + - obj_dist_min_x[2]**2) + obj_dist_min_x[0] ** 2 + + obj_dist_min_x[1] ** 2 + + obj_dist_min_x[2] ** 2 + ) # append class index, min x and min abs y to output array distance_output.append(float(cls)) @@ -399,36 +410,35 @@ def predict_ultralytics(self, image): # add values for visualization c_boxes.append(torch.tensor(pixels)) - c_labels.append(f"Class: {cls}," - f"Meters: {round(abs_distance, 2)}," - f"({round(float(obj_dist_min_x[0]), 2)}," - f"{round(float(obj_dist_min_abs_y[1]), 2)})") + c_labels.append( + f"Class: {cls}," + f"Meters: {round(abs_distance, 2)}," + f"({round(float(obj_dist_min_x[0]), 2)}," + f"{round(float(obj_dist_min_abs_y[1]), 2)})" + ) # publish list of distances of objects for planning - self.distance_publisher.publish( - Float32MultiArray(data=distance_output)) + self.distance_publisher.publish(Float32MultiArray(data=distance_output)) # transform image transposed_image = np.transpose(cv_image, (2, 0, 1)) - image_np_with_detections = torch.tensor(transposed_image, - dtype=torch.uint8) + image_np_with_detections = torch.tensor(transposed_image, dtype=torch.uint8) # proceed with traffic light detection if 9 in output[0].boxes.cls: - asyncio.run(self.process_traffic_lights(output[0], - cv_image, - image.header)) + asyncio.run(self.process_traffic_lights(output[0], cv_image, image.header)) # draw bounding boxes and distance values on image c_boxes = torch.stack(c_boxes) - box = draw_bounding_boxes(image_np_with_detections, - c_boxes, - c_labels, - colors='blue', - width=3, - font_size=12) - np_box_img = np.transpose(box.detach().numpy(), - (1, 2, 0)) + box = draw_bounding_boxes( + image_np_with_detections, + c_boxes, + c_labels, + colors="blue", + width=3, + font_size=12, + ) + np_box_img = np.transpose(box.detach().numpy(), (1, 2, 0)) box_img = cv2.cvtColor(np_box_img, cv2.COLOR_BGR2RGB) return box_img @@ -444,11 +454,8 @@ def min_x(self, dist_array): np.array: 1x3 numpy array of min x lidar point """ - min_x_sorted_indices = np.argsort( - dist_array[:, :, 0], - axis=None) - x, y = np.unravel_index(min_x_sorted_indices[0], - dist_array.shape[:2]) + min_x_sorted_indices = np.argsort(dist_array[:, :, 0], axis=None) + x, y = np.unravel_index(min_x_sorted_indices[0], dist_array.shape[:2]) return dist_array[x][y].copy() def min_abs_y(self, dist_array): @@ -464,11 +471,8 @@ def min_abs_y(self, dist_array): """ abs_distance_copy = np.abs(dist_array.copy()) - min_y_sorted_indices = np.argsort( - abs_distance_copy[:, :, 1], - axis=None) - x, y = np.unravel_index(min_y_sorted_indices[0], - abs_distance_copy.shape[:2]) + min_y_sorted_indices = np.argsort(abs_distance_copy[:, :, 1], axis=None) + x, y = np.unravel_index(min_y_sorted_indices[0], abs_distance_copy.shape[:2]) return dist_array[x][y].copy() # you can add similar functions to support other camera angles here @@ -493,12 +497,11 @@ async def process_traffic_lights(self, prediction, cv_image, image_header): continue box = box[0:4].astype(int) - segmented = cv_image[box[1]:box[3], box[0]:box[2]] + segmented = cv_image[box[1] : box[3], box[0] : box[2]] traffic_light_y_distance = box[1] - traffic_light_image = self.bridge.cv2_to_imgmsg(segmented, - encoding="rgb8") + traffic_light_image = self.bridge.cv2_to_imgmsg(segmented, encoding="rgb8") traffic_light_image.header = image_header traffic_light_image.header.frame_id = str(traffic_light_y_distance) self.traffic_light_publisher.publish(traffic_light_image) @@ -524,9 +527,9 @@ def create_mask(self, input_image, model_output): transposed_image = np.transpose(input_image, (2, 0, 1)) tensor_image = torch.tensor(transposed_image) tensor_image = tensor_image.to(dtype=torch.uint8) - segmented_image = draw_segmentation_masks(tensor_image, - output_predictions, - alpha=0.6) + segmented_image = draw_segmentation_masks( + tensor_image, output_predictions, alpha=0.6 + ) cv_segmented = segmented_image.detach().cpu().numpy() cv_segmented = np.transpose(cv_segmented, (1, 2, 0)) return cv_segmented @@ -544,21 +547,20 @@ def apply_bounding_boxes(self, input_image, model_output): """ transposed_image = np.transpose(input_image, (2, 0, 1)) - image_np_with_detections = torch.tensor(transposed_image, - dtype=torch.uint8) - boxes = model_output['boxes'] - labels = [self.weights.meta["categories"][i] - for i in model_output['labels']] - - box = draw_bounding_boxes(image_np_with_detections, - boxes, - labels, - colors='blue', - width=3, - font_size=24) - - np_box_img = np.transpose(box.detach().numpy(), - (1, 2, 0)) + image_np_with_detections = torch.tensor(transposed_image, dtype=torch.uint8) + boxes = model_output["boxes"] + labels = [self.weights.meta["categories"][i] for i in model_output["labels"]] + + box = draw_bounding_boxes( + image_np_with_detections, + boxes, + labels, + colors="blue", + width=3, + font_size=24, + ) + + np_box_img = np.transpose(box.detach().numpy(), (1, 2, 0)) box_img = cv2.cvtColor(np_box_img, cv2.COLOR_BGR2RGB) return box_img diff --git a/code/planning/setup.py b/code/planning/setup.py index dcc9fc08..d0f57539 100755 --- a/code/planning/setup.py +++ b/code/planning/setup.py @@ -2,6 +2,5 @@ from distutils.core import setup from catkin_pkg.python_setup import generate_distutils_setup -setup_args = generate_distutils_setup(packages=['planning'], - package_dir={'': 'src'}) +setup_args = generate_distutils_setup(packages=["planning"], package_dir={"": "src"}) setup(**setup_args) diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index eda141fe..48791e52 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -20,76 +20,108 @@ def grow_a_tree(role_name): rules = Parallel( "Rules", children=[ - Selector - ("Priorities", + Selector( + "Priorities", children=[ behaviours.maneuvers.UnstuckRoutine("Unstuck Routine"), - Selector("Road Features", - children=[ - behaviours.maneuvers.LeaveParkingSpace("Leave Parking Space"), - Sequence("Intersection", - children=[ - behaviours.road_features.IntersectionAhead - ("Intersection Ahead?"), - Sequence("Intersection Actions", - children=[ - behaviours.intersection.Approach - ("Approach Intersection"), - behaviours.intersection.Wait - ("Wait Intersection"), - behaviours.intersection.Enter - ("Enter Intersection"), - behaviours.intersection.Leave - ("Leave Intersection") - ]) - ]), - ]), - Selector("Laneswitching", children=[ - Sequence("Laneswitch", + Selector( + "Road Features", + children=[ + behaviours.maneuvers.LeaveParkingSpace( + "Leave Parking Space" + ), + Sequence( + "Intersection", children=[ - behaviours.road_features.LaneChangeAhead - ("Lane Change Ahead?"), - Sequence("Lane Change Actions", - children=[ - behaviours.lane_change.Approach - ("Approach Change"), - behaviours.lane_change.Wait - ("Wait Change"), - behaviours.lane_change.Enter - ("Enter Change"), - behaviours.lane_change.Leave - ("Leave Change") - ]) - ]), - Sequence("Overtaking", + behaviours.road_features.IntersectionAhead( + "Intersection Ahead?" + ), + Sequence( + "Intersection Actions", + children=[ + behaviours.intersection.Approach( + "Approach Intersection" + ), + behaviours.intersection.Wait( + "Wait Intersection" + ), + behaviours.intersection.Enter( + "Enter Intersection" + ), + behaviours.intersection.Leave( + "Leave Intersection" + ), + ], + ), + ], + ), + ], + ), + Selector( + "Laneswitching", + children=[ + Sequence( + "Laneswitch", children=[ - behaviours.road_features.OvertakeAhead - ("Overtake Ahead?"), - Sequence("Overtake Actions", - children=[ - behaviours.overtake.Approach - ("Approach Overtake"), - behaviours.overtake.Wait - ("Wait Overtake"), - behaviours.overtake.Enter - ("Enter Overtake"), - behaviours.overtake.Leave - ("Leave Overtake") - ]) - ]), - - ]), - behaviours.maneuvers.Cruise("Cruise") - ]) - ]) + behaviours.road_features.LaneChangeAhead( + "Lane Change Ahead?" + ), + Sequence( + "Lane Change Actions", + children=[ + behaviours.lane_change.Approach( + "Approach Change" + ), + behaviours.lane_change.Wait("Wait Change"), + behaviours.lane_change.Enter( + "Enter Change" + ), + behaviours.lane_change.Leave( + "Leave Change" + ), + ], + ), + ], + ), + Sequence( + "Overtaking", + children=[ + behaviours.road_features.OvertakeAhead( + "Overtake Ahead?" + ), + Sequence( + "Overtake Actions", + children=[ + behaviours.overtake.Approach( + "Approach Overtake" + ), + behaviours.overtake.Wait("Wait Overtake"), + behaviours.overtake.Enter("Enter Overtake"), + behaviours.overtake.Leave("Leave Overtake"), + ], + ), + ], + ), + ], + ), + behaviours.maneuvers.Cruise("Cruise"), + ], + ) + ], + ) - metarules = Sequence("Meta", children=[behaviours.meta.Start("Start"), rules, - behaviours.meta.End("End")]) - root = Parallel("Root", children=[ - behaviours.topics2blackboard.create_node(role_name), - metarules, - Running("Idle") - ]) + metarules = Sequence( + "Meta", + children=[behaviours.meta.Start("Start"), rules, behaviours.meta.End("End")], + ) + root = Parallel( + "Root", + children=[ + behaviours.topics2blackboard.create_node(role_name), + metarules, + Running("Idle"), + ], + ) return root @@ -101,7 +133,7 @@ def main(): """ Entry point for the demo script. """ - rospy.init_node('behavior_tree', anonymous=True) + rospy.init_node("behavior_tree", anonymous=True) role_name = rospy.get_param("~role_name", "hero") root = grow_a_tree(role_name) behaviour_tree = py_trees_ros.trees.BehaviourTree(root) @@ -119,5 +151,6 @@ def main(): except rospy.ROSInterruptException: pass + if __name__ == "__main__": main() diff --git a/code/planning/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/src/behavior_agent/behaviours/behavior_speed.py index 78edaf54..e5c93dea 100755 --- a/code/planning/src/behavior_agent/behaviours/behavior_speed.py +++ b/code/planning/src/behavior_agent/behaviours/behavior_speed.py @@ -1,4 +1,3 @@ - from collections import namedtuple diff --git a/code/planning/src/behavior_agent/behaviours/intersection.py b/code/planning/src/behavior_agent/behaviours/intersection.py index 48c2e357..6f2ce875 100755 --- a/code/planning/src/behavior_agent/behaviours/intersection.py +++ b/code/planning/src/behavior_agent/behaviours/intersection.py @@ -4,7 +4,7 @@ import rospy -from .import behavior_speed as bs +from . import behavior_speed as bs import planning # noqa: F401 @@ -34,6 +34,7 @@ class Approach(py_trees.behaviour.Behaviour): triggered. It than handles the approaching the intersection, slowing the vehicle down appropriately. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -55,9 +56,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", - String, queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -77,7 +78,7 @@ def initialise(self): self.traffic_light_detected = False self.traffic_light_distance = np.inf - self.traffic_light_status = '' + self.traffic_light_status = "" self.virtual_stopline_distance = np.inf @@ -100,10 +101,10 @@ def update(self): detected. """ # Update Light Info - light_status_msg = self.blackboard.get( - "/paf/hero/Center/traffic_light_state") + light_status_msg = self.blackboard.get("/paf/hero/Center/traffic_light_state") light_distance_y_msg = self.blackboard.get( - "/paf/hero/Center/traffic_light_y_distance") + "/paf/hero/Center/traffic_light_y_distance" + ) if light_status_msg is not None: self.traffic_light_status = get_color(light_status_msg.state) self.traffic_light_detected = True @@ -133,18 +134,19 @@ def update(self): target_distance = TARGET_DISTANCE_TO_STOP # stop when there is no or red/yellow traffic light or a stop sign is # detected - if self.traffic_light_status == '' \ - or self.traffic_light_status == 'red' \ - or self.traffic_light_status == 'yellow'\ - or (self.stop_sign_detected and - not self.traffic_light_detected): + if ( + self.traffic_light_status == "" + or self.traffic_light_status == "red" + or self.traffic_light_status == "yellow" + or (self.stop_sign_detected and not self.traffic_light_detected) + ): rospy.loginfo("slowing down!") self.curr_behavior_pub.publish(bs.int_app_to_stop.name) # approach slowly when traffic light is green as traffic lights are # higher priority than traffic signs this behavior is desired - if self.traffic_light_status == 'green': + if self.traffic_light_status == "green": self.curr_behavior_pub.publish(bs.int_app_green.name) # get speed @@ -154,30 +156,35 @@ def update(self): else: rospy.logwarn("no speedometer connected") return py_trees.common.Status.RUNNING - if (self.virtual_stopline_distance > target_distance) and \ - (self.traffic_light_distance > 150): + if (self.virtual_stopline_distance > target_distance) and ( + self.traffic_light_distance > 150 + ): # too far print("still approaching") return py_trees.common.Status.RUNNING - elif speed < convert_to_ms(2.0) and \ - ((self.virtual_stopline_distance < target_distance) or - (self.traffic_light_distance < 150)): + elif speed < convert_to_ms(2.0) and ( + (self.virtual_stopline_distance < target_distance) + or (self.traffic_light_distance < 150) + ): # stopped print("stopped") return py_trees.common.Status.SUCCESS - elif speed > convert_to_ms(5.0) and \ - self.virtual_stopline_distance < 6.0 and \ - self.traffic_light_status == "green": + elif ( + speed > convert_to_ms(5.0) + and self.virtual_stopline_distance < 6.0 + and self.traffic_light_status == "green" + ): # drive through intersection even if traffic light turns yellow return py_trees.common.Status.SUCCESS - elif speed > convert_to_ms(5.0) and \ - self.virtual_stopline_distance < 3.5: + elif speed > convert_to_ms(5.0) and self.virtual_stopline_distance < 3.5: # running over line return py_trees.common.Status.SUCCESS - if self.virtual_stopline_distance < target_distance and \ - not self.stopline_detected: + if ( + self.virtual_stopline_distance < target_distance + and not self.stopline_detected + ): rospy.loginfo("Leave intersection!") return py_trees.common.Status.SUCCESS else: @@ -194,9 +201,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Wait(py_trees.behaviour.Behaviour): @@ -205,6 +212,7 @@ class Wait(py_trees.behaviour.Behaviour): section until there either is no traffic light, the traffic light is green or the intersection is clear. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -225,9 +233,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() self.red_light_flag = False self.green_light_time = None @@ -262,8 +270,7 @@ def update(self): py_trees.common.Status.SUCCESS, if the traffic light switched to green or no traffic light is detected """ - light_status_msg = self.blackboard.get( - "/paf/hero/Center/traffic_light_state") + light_status_msg = self.blackboard.get("/paf/hero/Center/traffic_light_state") # ADD FEATURE: Check if intersection is clear lidar_data = None @@ -277,34 +284,36 @@ def update(self): if light_status_msg is not None: traffic_light_status = get_color(light_status_msg.state) - if traffic_light_status == "red" or \ - traffic_light_status == "yellow": + if traffic_light_status == "red" or traffic_light_status == "yellow": # Wait at traffic light self.red_light_flag = True self.green_light_time = rospy.get_rostime() rospy.loginfo(f"Light Status: {traffic_light_status}") self.curr_behavior_pub.publish(bs.int_wait.name) return py_trees.common.Status.RUNNING - elif rospy.get_rostime() - self.green_light_time < \ - rospy.Duration(1)\ - and traffic_light_status == "green": + elif ( + rospy.get_rostime() - self.green_light_time < rospy.Duration(1) + and traffic_light_status == "green" + ): # Wait approx 1s for confirmation rospy.loginfo("Confirm green light!") return py_trees.common.Status.RUNNING elif self.red_light_flag and traffic_light_status != "green": - rospy.loginfo(f"Light Status: {traffic_light_status}" - "-> prev was red") + rospy.loginfo(f"Light Status: {traffic_light_status}" "-> prev was red") # Probably some interference return py_trees.common.Status.RUNNING - elif rospy.get_rostime() - self.green_light_time > \ - rospy.Duration(1)\ - and traffic_light_status == "green": + elif ( + rospy.get_rostime() - self.green_light_time > rospy.Duration(1) + and traffic_light_status == "green" + ): rospy.loginfo(f"Light Status: {traffic_light_status}") # Drive through intersection return py_trees.common.Status.SUCCESS else: - rospy.loginfo(f"Light Status: {traffic_light_status}" - "-> No Traffic Light detected") + rospy.loginfo( + f"Light Status: {traffic_light_status}" + "-> No Traffic Light detected" + ) # Check clear if no traffic light is detected if not intersection_clear: @@ -326,9 +335,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Enter(py_trees.behaviour.Behaviour): @@ -337,6 +346,7 @@ class Enter(py_trees.behaviour.Behaviour): sets a speed and finishes if the ego vehicle is close to the end of the intersection. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -357,9 +367,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -416,9 +426,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Leave(py_trees.behaviour.Behaviour): @@ -426,6 +436,7 @@ class Leave(py_trees.behaviour.Behaviour): This behaviour defines the leaf of this subtree, if this behavior is reached, the vehicle left the intersection. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -446,9 +457,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -489,6 +500,6 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) diff --git a/code/planning/src/behavior_agent/behaviours/lane_change.py b/code/planning/src/behavior_agent/behaviours/lane_change.py index 9a6655b8..cf63419d 100755 --- a/code/planning/src/behavior_agent/behaviours/lane_change.py +++ b/code/planning/src/behavior_agent/behaviours/lane_change.py @@ -19,6 +19,7 @@ class Approach(py_trees.behaviour.Behaviour): triggered. It than handles approaching the lane change, slowing the vehicle down appropriately. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -40,9 +41,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", - String, queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -116,13 +117,19 @@ def update(self): rospy.loginfo("still approaching") self.curr_behavior_pub.publish(bs.lc_app_blocked.name) return py_trees.common.Status.RUNNING - elif speed < convert_to_ms(2.0) and \ - self.virtual_change_distance < target_dis and self.blocked: + elif ( + speed < convert_to_ms(2.0) + and self.virtual_change_distance < target_dis + and self.blocked + ): # stopped rospy.loginfo("stopped") return py_trees.common.Status.SUCCESS - elif speed > convert_to_ms(5.0) and \ - self.virtual_change_distance < 3.5 and not self.blocked: + elif ( + speed > convert_to_ms(5.0) + and self.virtual_change_distance < 3.5 + and not self.blocked + ): # running over line return py_trees.common.Status.SUCCESS else: @@ -139,15 +146,16 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Wait(py_trees.behaviour.Behaviour): """ This behavior handles the waiting in front of the lane change. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -168,9 +176,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -241,9 +249,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Enter(py_trees.behaviour.Behaviour): @@ -252,6 +260,7 @@ class Enter(py_trees.behaviour.Behaviour): sets a speed and finishes if the ego vehicle is close to the end of the intersection. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -272,9 +281,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -307,8 +316,7 @@ def update(self): py_trees.common.Status.FAILURE, if no next path point can be detected. """ - next_waypoint_msg = self.blackboard.\ - get("/paf/hero/lane_change_distance") + next_waypoint_msg = self.blackboard.get("/paf/hero/lane_change_distance") if next_waypoint_msg is None: return py_trees.common.Status.FAILURE @@ -329,9 +337,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Leave(py_trees.behaviour.Behaviour): @@ -339,6 +347,7 @@ class Leave(py_trees.behaviour.Behaviour): This behaviour defines the leaf of this subtree, if this behavior is reached, the vehicle left the intersection. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -359,9 +368,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -404,6 +413,6 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 2a37f7a1..913fa0c0 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -3,6 +3,7 @@ from std_msgs.msg import String, Float32, Bool import numpy as np from . import behavior_speed as bs + # from behavior_agent.msg import BehaviorSpeed """ @@ -15,6 +16,7 @@ class LeaveParkingSpace(py_trees.behaviour.Behaviour): This behavior is triggered in the beginning when the vehicle needs to leave the parking space. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -40,9 +42,9 @@ def setup(self, timeout): successful :return: True, as there is nothing to set up. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", - String, queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() self.initPosition = None return True @@ -90,13 +92,20 @@ def update(self): speed = self.blackboard.get("/carla/hero/Speed") if self.called is False: # calculate distance between start and current position - if position is not None and \ - self.initPosition is not None and \ - speed is not None: - startPos = np.array([position.pose.position.x, - position.pose.position.y]) - endPos = np.array([self.initPosition.pose.position.x, - self.initPosition.pose.position.y]) + if ( + position is not None + and self.initPosition is not None + and speed is not None + ): + startPos = np.array( + [position.pose.position.x, position.pose.position.y] + ) + endPos = np.array( + [ + self.initPosition.pose.position.x, + self.initPosition.pose.position.y, + ] + ) distance = np.linalg.norm(startPos - endPos) if distance < 1 or speed.speed < 2: self.curr_behavior_pub.publish(bs.parking.name) @@ -121,8 +130,10 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class SwitchLaneLeft(py_trees.behaviour.Behaviour): @@ -131,6 +142,7 @@ class SwitchLaneLeft(py_trees.behaviour.Behaviour): switch to the lane to the left. A check if the lane is free might be added in the future. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -205,8 +217,10 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class SwitchLaneRight(py_trees.behaviour.Behaviour): @@ -215,6 +229,7 @@ class SwitchLaneRight(py_trees.behaviour.Behaviour): switch to the lane to the right. A check if the lane is free might be added in the future. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -281,8 +296,10 @@ def update(self): return py_trees.common.Status.RUNNING def terminate(self, new_status): - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Cruise(py_trees.behaviour.Behaviour): @@ -296,6 +313,7 @@ class Cruise(py_trees.behaviour.Behaviour): speed control = acting via speed limits and target_speed following the trajectory = acting """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -320,9 +338,9 @@ def setup(self, timeout): :return: True, as there is nothing to set up. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", - String, queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -369,8 +387,10 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) def get_distance(pos_1, pos_2): @@ -402,7 +422,6 @@ def pos_to_np_array(pos): class UnstuckRoutine(py_trees.behaviour.Behaviour): - """ Documentation to this behavior can be found in /doc/planning/Behavior_detailed.md @@ -411,6 +430,7 @@ class UnstuckRoutine(py_trees.behaviour.Behaviour): unstuck. The behavior will then try to reverse and steer to the left or right to get out of the stuck situation. """ + def reset_stuck_values(self): self.unstuck_overtake_count = 0 self.stuck_timer = rospy.Time.now() @@ -421,16 +441,20 @@ def print_warnings(self): self.last_stuck_duration_log = self.stuck_duration self.last_wait_stuck_duration_log = self.wait_stuck_duration - stuck_duration_diff = (self.stuck_duration - - self.last_stuck_duration_log) - wait_stuck_duration_diff = (self.wait_stuck_duration - - self.last_wait_stuck_duration_log) + stuck_duration_diff = self.stuck_duration - self.last_stuck_duration_log + wait_stuck_duration_diff = ( + self.wait_stuck_duration - self.last_wait_stuck_duration_log + ) - if self.stuck_duration.secs > TRIGGER_STUCK_DURATION.secs/2 \ - and stuck_duration_diff.secs >= 1: + if ( + self.stuck_duration.secs > TRIGGER_STUCK_DURATION.secs / 2 + and stuck_duration_diff.secs >= 1 + ): rospy.logwarn(f"Stuck for {self.stuck_duration.secs} s") - if self.wait_stuck_duration.secs > TRIGGER_WAIT_STUCK_DURATION.secs/2\ - and wait_stuck_duration_diff.secs >= 1: + if ( + self.wait_stuck_duration.secs > TRIGGER_WAIT_STUCK_DURATION.secs / 2 + and wait_stuck_duration_diff.secs >= 1 + ): rospy.logwarn(f"Wait stuck for {self.wait_stuck_duration.secs} s") def __init__(self, name): @@ -469,15 +493,15 @@ def setup(self, timeout): successful :return: True, as there is nothing to set up. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", - String, queue_size=1) - self.pub_unstuck_distance = rospy.Publisher("/paf/hero/" - "unstuck_distance", - Float32, queue_size=1) - self.pub_unstuck_flag = rospy.Publisher("/paf/hero/" - "unstuck_flag", - Bool, queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) + self.pub_unstuck_distance = rospy.Publisher( + "/paf/hero/" "unstuck_distance", Float32, queue_size=1 + ) + self.pub_unstuck_flag = rospy.Publisher( + "/paf/hero/" "unstuck_flag", Bool, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -527,18 +551,24 @@ def initialise(self): # print fatal error if stuck for too long if self.stuck_duration >= TRIGGER_STUCK_DURATION: - rospy.logfatal(f"""Should be Driving but Stuck in one place + rospy.logfatal( + f"""Should be Driving but Stuck in one place for more than {TRIGGER_STUCK_DURATION.secs}\n - -> starting unstuck routine""") + -> starting unstuck routine""" + ) self.init_pos = pos_to_np_array( - self.blackboard.get("/paf/hero/current_pos")) + self.blackboard.get("/paf/hero/current_pos") + ) elif self.wait_stuck_duration >= TRIGGER_WAIT_STUCK_DURATION: - rospy.logfatal(f"""Wait Stuck in one place + rospy.logfatal( + f"""Wait Stuck in one place for more than {TRIGGER_WAIT_STUCK_DURATION.secs} \n - -> starting unstuck routine""") + -> starting unstuck routine""" + ) self.init_pos = pos_to_np_array( - self.blackboard.get("/paf/hero/current_pos")) + self.blackboard.get("/paf/hero/current_pos") + ) return True @@ -563,15 +593,16 @@ def update(self): # self.stuck_timer = rospy.Time.now() # self.wait_stuck_timer = rospy.Time.now() - self.current_pos = pos_to_np_array( - self.blackboard.get("/paf/hero/current_pos")) + self.current_pos = pos_to_np_array(self.blackboard.get("/paf/hero/current_pos")) self.current_speed = self.blackboard.get("/carla/hero/Speed") if self.init_pos is None or self.current_pos is None: return py_trees.common.Status.FAILURE # if no stuck detected, return failure - if self.stuck_duration < TRIGGER_STUCK_DURATION and \ - self.wait_stuck_duration < TRIGGER_WAIT_STUCK_DURATION: + if ( + self.stuck_duration < TRIGGER_STUCK_DURATION + and self.wait_stuck_duration < TRIGGER_WAIT_STUCK_DURATION + ): # rospy.logfatal("No stuck detected.") self.pub_unstuck_flag.publish(False) # unstuck distance -1 is set, to reset the unstuck distance @@ -579,7 +610,7 @@ def update(self): return py_trees.common.Status.FAILURE # stuck detected -> unstuck routine - if rospy.Time.now()-self.init_ros_stuck_time < UNSTUCK_DRIVE_DURATION: + if rospy.Time.now() - self.init_ros_stuck_time < UNSTUCK_DRIVE_DURATION: self.curr_behavior_pub.publish(bs.us_unstuck.name) self.pub_unstuck_flag.publish(True) rospy.logfatal("Unstuck routine running.") @@ -590,21 +621,20 @@ def update(self): self.curr_behavior_pub.publish(bs.us_stop.name) return py_trees.common.Status.RUNNING # vehicle has stopped: - unstuck_distance = get_distance(self.init_pos, - self.current_pos) + unstuck_distance = get_distance(self.init_pos, self.current_pos) self.pub_unstuck_distance.publish(unstuck_distance) # check if vehicle needs to overtake: # save current pos to last_unstuck_positions - self.last_unstuck_positions = np.roll(self.last_unstuck_positions, - -1, axis=0) + self.last_unstuck_positions = np.roll( + self.last_unstuck_positions, -1, axis=0 + ) self.last_unstuck_positions[-1] = self.init_pos # if last unstuck was too far away, no overtake # we only want to overtake when we tried to unstuck twice # this case is the first time ever we tried to unstuck - if np.array_equal(self.last_unstuck_positions[0], - np.array([0, 0])): + if np.array_equal(self.last_unstuck_positions[0], np.array([0, 0])): self.reset_stuck_values() rospy.logwarn("Unstuck routine finished.") return py_trees.common.Status.FAILURE @@ -614,9 +644,12 @@ def update(self): # if the distance between the last and the first unstuck position # is too far, we don't want to overtake, since its the first # unstuck routine at this position on the map - if get_distance(self.last_unstuck_positions[0], - self.last_unstuck_positions[-1])\ - > UNSTUCK_CLEAR_DISTANCE: + if ( + get_distance( + self.last_unstuck_positions[0], self.last_unstuck_positions[-1] + ) + > UNSTUCK_CLEAR_DISTANCE + ): self.reset_stuck_values() rospy.logwarn("Unstuck routine finished.") return py_trees.common.Status.FAILURE @@ -646,5 +679,7 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) diff --git a/code/planning/src/behavior_agent/behaviours/meta.py b/code/planning/src/behavior_agent/behaviours/meta.py index c3921d1a..2461495c 100755 --- a/code/planning/src/behavior_agent/behaviours/meta.py +++ b/code/planning/src/behavior_agent/behaviours/meta.py @@ -14,6 +14,7 @@ class Start(py_trees.behaviour.Behaviour): This behavior is the first one being called when the decision tree starts, it sets a first target_speed """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -35,9 +36,9 @@ def setup(self, timeout): :return: True, as the set up is successful. """ self.blackboard = py_trees.blackboard.Blackboard() - self.target_speed_pub = rospy.Publisher("paf/hero/" - "max_velocity", - Float32, queue_size=1) + self.target_speed_pub = rospy.Publisher( + "paf/hero/" "max_velocity", Float32, queue_size=1 + ) return True def initialise(self): @@ -76,14 +77,17 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates :param new_status: new state after this one is terminated """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class End(py_trees.behaviour.Behaviour): """ This behavior is called as the last one when the agent finished the path. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -105,9 +109,9 @@ def setup(self, timeout): :return: True, as the set up is successful. """ self.blackboard = py_trees.blackboard.Blackboard() - self.target_speed_pub = rospy.Publisher("/paf/hero/" - "max_velocity", - Float32, queue_size=1) + self.target_speed_pub = rospy.Publisher( + "/paf/hero/" "max_velocity", Float32, queue_size=1 + ) return True def initialise(self): @@ -150,5 +154,7 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates :param new_status: new state after this one is terminated """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) diff --git a/code/planning/src/behavior_agent/behaviours/overtake.py b/code/planning/src/behavior_agent/behaviours/overtake.py index 5b1bb7b4..affcbc61 100644 --- a/code/planning/src/behavior_agent/behaviours/overtake.py +++ b/code/planning/src/behavior_agent/behaviours/overtake.py @@ -6,8 +6,7 @@ from . import behavior_speed as bs import planning # noqa: F401 -from local_planner.utils import NUM_WAYPOINTS, TARGET_DISTANCE_TO_STOP, \ - convert_to_ms +from local_planner.utils import NUM_WAYPOINTS, TARGET_DISTANCE_TO_STOP, convert_to_ms """ Source: https://github.com/ll7/psaf2 @@ -25,6 +24,7 @@ class Approach(py_trees.behaviour.Behaviour): behaviours.road_features.overtake_ahead is triggered. It than handles the procedure for overtaking. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -46,9 +46,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", - String, queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -103,8 +103,10 @@ def update(self): else: distance_oncoming = 35 - if distance_oncoming is not None and \ - distance_oncoming > self.clear_distance: + if ( + distance_oncoming is not None + and distance_oncoming > self.clear_distance + ): rospy.loginfo("Overtake is free not slowing down!") self.curr_behavior_pub.publish(bs.ot_app_free.name) return py_trees.common.Status.SUCCESS @@ -124,8 +126,7 @@ def update(self): # too far rospy.loginfo("still approaching") return py_trees.common.Status.RUNNING - elif speed < convert_to_ms(2.0) and \ - self.ot_distance < TARGET_DISTANCE_TO_STOP: + elif speed < convert_to_ms(2.0) and self.ot_distance < TARGET_DISTANCE_TO_STOP: # stopped rospy.loginfo("stopped") return py_trees.common.Status.SUCCESS @@ -144,9 +145,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Wait(py_trees.behaviour.Behaviour): @@ -155,6 +156,7 @@ class Wait(py_trees.behaviour.Behaviour): which is blocking the road. The Ego vehicle is waiting to get a clear path for overtaking. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -175,9 +177,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -251,9 +253,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Enter(py_trees.behaviour.Behaviour): @@ -261,6 +263,7 @@ class Enter(py_trees.behaviour.Behaviour): This behavior handles the switching to a new lane in the overtaking procedure. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -281,9 +284,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -342,9 +345,9 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class Leave(py_trees.behaviour.Behaviour): @@ -352,6 +355,7 @@ class Leave(py_trees.behaviour.Behaviour): This behaviour defines the leaf of this subtree, if this behavior is reached, the vehicle peformed the overtake. """ + def __init__(self, name): """ Minimal one-time initialisation. Other one-time initialisation @@ -372,9 +376,9 @@ def setup(self, timeout): successful :return: True, as the set up is successful. """ - self.curr_behavior_pub = rospy.Publisher("/paf/hero/" - "curr_behavior", String, - queue_size=1) + self.curr_behavior_pub = rospy.Publisher( + "/paf/hero/" "curr_behavior", String, queue_size=1 + ) self.blackboard = py_trees.blackboard.Blackboard() return True @@ -389,8 +393,7 @@ def initialise(self): """ self.curr_behavior_pub.publish(bs.ot_leave.name) data = self.blackboard.get("/paf/hero/current_pos") - self.first_pos = np.array([data.pose.position.x, - data.pose.position.y]) + self.first_pos = np.array([data.pose.position.x, data.pose.position.y]) rospy.loginfo(f"Leave Overtake: {self.first_pos}") return True @@ -407,8 +410,7 @@ def update(self): """ global OVERTAKE_EXECUTING data = self.blackboard.get("/paf/hero/current_pos") - self.current_pos = np.array([data.pose.position.x, - data.pose.position.y]) + self.current_pos = np.array([data.pose.position.x, data.pose.position.y]) distance = np.linalg.norm(self.first_pos - self.current_pos) if distance > OVERTAKE_EXECUTING + NUM_WAYPOINTS: rospy.loginfo(f"Left Overtake: {self.current_pos}") @@ -427,6 +429,6 @@ def terminate(self, new_status): :param new_status: new state after this one is terminated """ self.logger.debug( - " %s [Foo::terminate().terminate()][%s->%s]" % (self.name, - self.status, - new_status)) + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) diff --git a/code/planning/src/behavior_agent/behaviours/road_features.py b/code/planning/src/behavior_agent/behaviours/road_features.py index 67a70a36..13f00caf 100755 --- a/code/planning/src/behavior_agent/behaviours/road_features.py +++ b/code/planning/src/behavior_agent/behaviours/road_features.py @@ -17,6 +17,7 @@ class IntersectionAhead(py_trees.behaviour.Behaviour): ego vehicle or not and triggers the rest of the decision tree handling the intersection. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -91,8 +92,10 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates :param new_status: new state after this one is terminated """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class LaneChangeAhead(py_trees.behaviour.Behaviour): @@ -101,6 +104,7 @@ class LaneChangeAhead(py_trees.behaviour.Behaviour): ego vehicle or not and triggers the rest of the decision tree handling the lane change. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -174,8 +178,10 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates :param new_status: new state after this one is terminated """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class OvertakeAhead(py_trees.behaviour.Behaviour): @@ -183,6 +189,7 @@ class OvertakeAhead(py_trees.behaviour.Behaviour): This behaviour checks whether an object that needs to be overtaken is ahead """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -241,13 +248,13 @@ def update(self): current_position = self.blackboard.get("/paf/hero/current_pos") current_heading = self.blackboard.get("/paf/hero/current_heading").data - if obstacle_msg is None or \ - current_position is None or \ - current_heading is None: + if obstacle_msg is None or current_position is None or current_heading is None: return py_trees.common.Status.FAILURE - current_position = [current_position.pose.position.x, - current_position.pose.position.y, - current_position.pose.position.z] + current_position = [ + current_position.pose.position.x, + current_position.pose.position.y, + current_position.pose.position.z, + ] obstacle_distance = obstacle_msg.data[0] obstacle_speed = obstacle_msg.data[1] @@ -255,11 +262,12 @@ def update(self): if obstacle_distance == np.Inf: return py_trees.common.Status.FAILURE # calculate approx collision position in global coords - rotation_matrix = Rotation.from_euler('z', current_heading) + rotation_matrix = Rotation.from_euler("z", current_heading) # Apply current heading to absolute distance vector # and add to current position pos_moved_in_x_direction = current_position + rotation_matrix.apply( - np.array([obstacle_distance, 0, 0])) + np.array([obstacle_distance, 0, 0]) + ) if np.linalg.norm(pos_moved_in_x_direction - current_position) < 1: # current collision is not near trajectory lane @@ -285,8 +293,10 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates :param new_status: new state after this one is terminated """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class MultiLane(py_trees.behaviour.Behaviour): @@ -295,6 +305,7 @@ class MultiLane(py_trees.behaviour.Behaviour): one lane in the driving direction. This could be used to change lanes to the right to perhaps evade an emergency vehicle. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -367,8 +378,10 @@ def terminate(self, new_status): down writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class SingleLineDotted(py_trees.behaviour.Behaviour): @@ -376,6 +389,7 @@ class SingleLineDotted(py_trees.behaviour.Behaviour): This behavior checks if it is allowed to switch lanes one a single lane street. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -445,8 +459,10 @@ def terminate(self, new_status): down writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class RightLaneAvailable(py_trees.behaviour.Behaviour): @@ -454,6 +470,7 @@ class RightLaneAvailable(py_trees.behaviour.Behaviour): This behavior checks if there is a lane to the right of the agent it could change to. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -522,8 +539,10 @@ def terminate(self, new_status): down writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class LeftLaneAvailable(py_trees.behaviour.Behaviour): @@ -531,6 +550,7 @@ class LeftLaneAvailable(py_trees.behaviour.Behaviour): On a multi-lane, this behavior checks if there is a lane to the left of the agent it could change to, to overtake. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -602,5 +622,7 @@ def terminate(self, new_status): down writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s ]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s ]" + % (self.name, self.status, new_status) + ) diff --git a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py index 46616694..5f80061b 100755 --- a/code/planning/src/behavior_agent/behaviours/topics2blackboard.py +++ b/code/planning/src/behavior_agent/behaviours/topics2blackboard.py @@ -24,50 +24,92 @@ def create_node(role_name): :return: topics2blackboard the subtree of the topics in the blackboard """ topics = [ - {'name': f"/carla/{role_name}/Speed", 'msg': CarlaSpeedometer, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/slowed_by_car_in_front", 'msg': Bool, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/waypoint_distance", 'msg': Waypoint, - 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, - {'name': f"/paf/{role_name}/stop_sign", 'msg': Stop_sign, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': - f"/paf/{role_name}/Center/traffic_light_state", - 'msg': TrafficLightState, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': - f"/paf/{role_name}/Center/traffic_light_y_distance", - 'msg': Int16, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/max_velocity", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/speed_limit", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/lane_change_distance", 'msg': LaneChange, - 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, - {'name': f"/paf/{role_name}/collision", 'msg': Float32MultiArray, - 'clearing-policy': py_trees.common.ClearingPolicy.ON_INITIALISE}, - {'name': f"/paf/{role_name}/current_pos", 'msg': PoseStamped, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/current_heading", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/overtake_success", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/oncoming", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER}, - {'name': f"/paf/{role_name}/target_velocity", 'msg': Float32, - 'clearing-policy': py_trees.common.ClearingPolicy.NEVER} + { + "name": f"/carla/{role_name}/Speed", + "msg": CarlaSpeedometer, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/slowed_by_car_in_front", + "msg": Bool, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/waypoint_distance", + "msg": Waypoint, + "clearing-policy": py_trees.common.ClearingPolicy.ON_INITIALISE, + }, + { + "name": f"/paf/{role_name}/stop_sign", + "msg": Stop_sign, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/Center/traffic_light_state", + "msg": TrafficLightState, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/Center/traffic_light_y_distance", + "msg": Int16, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/max_velocity", + "msg": Float32, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/speed_limit", + "msg": Float32, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/lane_change_distance", + "msg": LaneChange, + "clearing-policy": py_trees.common.ClearingPolicy.ON_INITIALISE, + }, + { + "name": f"/paf/{role_name}/collision", + "msg": Float32MultiArray, + "clearing-policy": py_trees.common.ClearingPolicy.ON_INITIALISE, + }, + { + "name": f"/paf/{role_name}/current_pos", + "msg": PoseStamped, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/current_heading", + "msg": Float32, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/overtake_success", + "msg": Float32, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/oncoming", + "msg": Float32, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, + { + "name": f"/paf/{role_name}/target_velocity", + "msg": Float32, + "clearing-policy": py_trees.common.ClearingPolicy.NEVER, + }, ] topics2blackboard = py_trees.composites.Parallel("Topics to Blackboard") for topic in topics: topics2blackboard.add_child( - py_trees_ros. - subscribers. - ToBlackboard(name=topic['name'], - topic_name=topic['name'], - topic_type=topic['msg'], - blackboard_variables={topic['name']: None}, - clearing_policy=topic['clearing-policy'])) + py_trees_ros.subscribers.ToBlackboard( + name=topic["name"], + topic_name=topic["name"], + topic_type=topic["msg"], + blackboard_variables={topic["name"]: None}, + clearing_policy=topic["clearing-policy"], + ) + ) return topics2blackboard diff --git a/code/planning/src/behavior_agent/behaviours/traffic_objects.py b/code/planning/src/behavior_agent/behaviours/traffic_objects.py index 547ebce9..d15efb64 100755 --- a/code/planning/src/behavior_agent/behaviours/traffic_objects.py +++ b/code/planning/src/behavior_agent/behaviours/traffic_objects.py @@ -14,6 +14,7 @@ class NotSlowedByCarInFront(py_trees.behaviour.Behaviour): More cases could be added later on. This behavior should be triggered by the perception. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -87,14 +88,17 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class WaitLeftLaneFree(py_trees.behaviour.Behaviour): """ This behavior checks if it is safe to change to the lane on the left. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -173,14 +177,17 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class WaitRightLaneFree(py_trees.behaviour.Behaviour): """ This behavior checks if it is safe to change to the lane on the left. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -257,8 +264,10 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class NotSlowedByCarInFrontRight(py_trees.behaviour.Behaviour): @@ -266,6 +275,7 @@ class NotSlowedByCarInFrontRight(py_trees.behaviour.Behaviour): Checks if there is a car on the lane to the right that would slow the agent down. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -336,14 +346,17 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) class OvertakingPossible(py_trees.behaviour.Behaviour): """ Checks if the overtaking is possible. """ + def __init__(self, name): """ Minimal one-time initialisation. A good rule of thumb is to only @@ -414,5 +427,7 @@ def terminate(self, new_status): writes a status message to the console when the behaviour terminates """ - self.logger.debug(" %s [Foo::terminate().terminate()][%s->%s]" % - (self.name, self.status, new_status)) + self.logger.debug( + " %s [Foo::terminate().terminate()][%s->%s]" + % (self.name, self.status, new_status) + ) diff --git a/code/planning/src/global_planner/dev_global_route.py b/code/planning/src/global_planner/dev_global_route.py index 1d455ac7..2f01fdf4 100755 --- a/code/planning/src/global_planner/dev_global_route.py +++ b/code/planning/src/global_planner/dev_global_route.py @@ -25,37 +25,38 @@ class DevGlobalRoute(CompatibleNode): def __init__(self): - super(DevGlobalRoute, self).__init__('DevGlobalRoute') + super(DevGlobalRoute, self).__init__("DevGlobalRoute") self.role_name = self.get_param("role_name", "hero") self.from_txt = self.get_param("from_txt", True) if self.from_txt: self.global_route_txt = self.get_param( - 'global_route_txt', - "/code/planning/src/global_planner/global_route.txt") + "global_route_txt", "/code/planning/src/global_planner/global_route.txt" + ) else: - self.sampling_resolution = self.get_param('sampling_resolution', - 100.0) + self.sampling_resolution = self.get_param("sampling_resolution", 100.0) # consecutively increasing sequence ID for header_msg self.seq = 0 self.routes = self.get_param( - 'routes', "/opt/leaderboard/data/routes_devtest.xml") + "routes", "/opt/leaderboard/data/routes_devtest.xml" + ) self.map_sub = self.new_subscription( msg_type=CarlaWorldInfo, topic="/carla/world_info", callback=self.world_info_callback, - qos_profile=10) + qos_profile=10, + ) self.global_plan_pub = self.new_publisher( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic="/carla/" + self.role_name + "/global_plan", qos_profile=QoSProfile( - depth=1, - durability=DurabilityPolicy.TRANSIENT_LOCAL) + depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL + ), ) - self.logerr('DevGlobalRoute-Node started') + self.logerr("DevGlobalRoute-Node started") def world_info_callback(self, data: CarlaWorldInfo) -> None: """ @@ -69,8 +70,10 @@ def world_info_callback(self, data: CarlaWorldInfo) -> None: with open(f"/workspace{self.global_route_txt}", "r") as txt: input_routes = txt.read() except FileNotFoundError: - self.logerr(f"/workspace{self.global_route_txt} not found... " - f"current working directory is'{os.getcwd()}'") + self.logerr( + f"/workspace{self.global_route_txt} not found... " + f"current working directory is'{os.getcwd()}'" + ) raise self.logerr("DevRoute: TXT READ") @@ -85,11 +88,11 @@ def world_info_callback(self, data: CarlaWorldInfo) -> None: secs = int(route.split("secs: ")[1].split("\n")[0]) nsecs = int(route.split("nsecs:")[1].split("\n")[0]) frame_id = route.split('"')[1] - header_list.append( - Header(seq, rospy.Time(secs, nsecs), frame_id)) + header_list.append(Header(seq, rospy.Time(secs, nsecs), frame_id)) road_options_str = route.split("[")[1].split("]")[0].split(",") - road_options_list.append([int(road_option) - for road_option in road_options_str]) + road_options_list.append( + [int(road_option) for road_option in road_options_str] + ) poses_str = route.split("position:")[1:] poses = [] for pose in poses_str: @@ -105,21 +108,21 @@ def world_info_callback(self, data: CarlaWorldInfo) -> None: orientation = Quaternion(x, y, z, w) poses.append(Pose(position, orientation)) poses_list.append(poses) - self.global_plan_pub.publish(header_list[0], road_options_list[0], - poses_list[0]) + self.global_plan_pub.publish( + header_list[0], road_options_list[0], poses_list[0] + ) else: - with open(self.routes, 'r', encoding='utf-8') as file: + with open(self.routes, "r", encoding="utf-8") as file: my_xml = file.read() # Use xmltodict to parse and convert the XML document routes_dict = xmltodict.parse(my_xml) - route = routes_dict['routes']['route'][0] - town = route['@town'] + route = routes_dict["routes"]["route"][0] + town = route["@town"] if town not in data.map_name: - self.logerr(f"Map '{data.map_name}' doesnt match routes " - f"'{town}'") + self.logerr(f"Map '{data.map_name}' doesnt match routes " f"'{town}'") return # Convert data into a carla.Map @@ -130,15 +133,15 @@ def world_info_callback(self, data: CarlaWorldInfo) -> None: # plan the route between given waypoints route_trace = [] - waypoints = route['waypoints']['position'] + waypoints = route["waypoints"]["position"] prepoint = waypoints[0] for waypoint in waypoints[1:]: - start = carla.Location(float(prepoint['@x']), - float(prepoint['@y']), - float(prepoint['@z'])) - origin = carla.Location(float(waypoint['@x']), - float(waypoint['@y']), - float(waypoint['@z'])) + start = carla.Location( + float(prepoint["@x"]), float(prepoint["@y"]), float(prepoint["@z"]) + ) + origin = carla.Location( + float(waypoint["@x"]), float(waypoint["@y"]), float(waypoint["@z"]) + ) part_route_trace = grp.trace_route(start, origin) route_trace.extend(part_route_trace) prepoint = waypoint @@ -152,9 +155,11 @@ def world_info_callback(self, data: CarlaWorldInfo) -> None: rotation = waypoint.transform.rotation quaternion = tf.transformations.quaternion_from_euler( - rotation.roll, rotation.pitch, rotation.yaw) - orientation = Quaternion(quaternion[0], quaternion[1], - quaternion[2], quaternion[3]) + rotation.roll, rotation.pitch, rotation.yaw + ) + orientation = Quaternion( + quaternion[0], quaternion[1], quaternion[2], quaternion[3] + ) poses.append(Pose(position, orientation)) road_options.append(road_option) @@ -169,10 +174,10 @@ def world_info_callback(self, data: CarlaWorldInfo) -> None: if __name__ == "__main__": """ - main function starts the NavManager node - :param args: + main function starts the NavManager node + :param args: """ - roscomp.init('DevGlobalRoute') + roscomp.init("DevGlobalRoute") try: DevGlobalRoute() diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index e7510ebf..291e161b 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -6,7 +6,7 @@ from xml.etree import ElementTree as eTree from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo +from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo from nav_msgs.msg import Path from std_msgs.msg import String from std_msgs.msg import Float32MultiArray @@ -34,7 +34,7 @@ class PrePlanner(CompatibleNode): """ def __init__(self): - super(PrePlanner, self).__init__('DevGlobalRoute') + super(PrePlanner, self).__init__("DevGlobalRoute") self.path_backup = Path() @@ -46,36 +46,42 @@ def __init__(self): self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) self.distance_spawn_to_first_wp = self.get_param( - "distance_spawn_to_first_wp", 100) + "distance_spawn_to_first_wp", 100 + ) self.map_sub = self.new_subscription( msg_type=String, topic=f"/carla/{self.role_name}/OpenDRIVE", callback=self.world_info_callback, - qos_profile=10) + qos_profile=10, + ) self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic="/carla/" + self.role_name + "/global_plan", callback=self.global_route_callback, - qos_profile=10) + qos_profile=10, + ) self.current_pos_sub = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.position_callback, - qos_profile=1) + qos_profile=1, + ) self.path_pub = self.new_publisher( msg_type=Path, - topic='/paf/' + self.role_name + '/trajectory_global', - qos_profile=1) + topic="/paf/" + self.role_name + "/trajectory_global", + qos_profile=1, + ) self.speed_limit_pub = self.new_publisher( msg_type=Float32MultiArray, topic=f"/paf/{self.role_name}/speed_limits_OpenDrive", - qos_profile=1) - self.logdebug('PrePlanner-Node started') + qos_profile=1, + ) + self.logdebug("PrePlanner-Node started") # uncomment for self.dev_load_world_info() for dev_launch # self.dev_load_world_info() @@ -92,26 +98,33 @@ def global_route_callback(self, data: CarlaRoute) -> None: return if self.odc is None: - self.logwarn("PrePlanner: global route got updated before map... " - "therefore the OpenDriveConverter couldn't be " - "initialised yet") + self.logwarn( + "PrePlanner: global route got updated before map... " + "therefore the OpenDriveConverter couldn't be " + "initialised yet" + ) self.global_route_backup = data return if self.agent_pos is None or self.agent_ori is None: - self.logwarn("PrePlanner: global route got updated before current " - "pose... therefore there is no pose to start with") + self.logwarn( + "PrePlanner: global route got updated before current " + "pose... therefore there is no pose to start with" + ) self.global_route_backup = data return - x_start = self.agent_pos.x # 983.5 - y_start = self.agent_pos.y # -5433.2 + x_start = self.agent_pos.x # 983.5 + y_start = self.agent_pos.y # -5433.2 x_target = data.poses[0].position.x y_target = data.poses[0].position.y - if abs(x_start - x_target) > self.distance_spawn_to_first_wp or \ - abs(y_start - y_target) > self.distance_spawn_to_first_wp: - self.logwarn("PrePlanner: current agent-pose doesnt match the " - "given global route") + if ( + abs(x_start - x_target) > self.distance_spawn_to_first_wp + or abs(y_start - y_target) > self.distance_spawn_to_first_wp + ): + self.logwarn( + "PrePlanner: current agent-pose doesnt match the " "given global route" + ) self.global_route_backup = data return @@ -128,35 +141,45 @@ def global_route_callback(self, data: CarlaRoute) -> None: x_target = None y_target = None - x_turn_follow = data.poses[ind+1].position.x - y_turn_follow = data.poses[ind+1].position.y + x_turn_follow = data.poses[ind + 1].position.x + y_turn_follow = data.poses[ind + 1].position.y # Trajectory for the starting road segment - self.odc.initial_road_trajectory(x_start, y_start, - x_turn, y_turn, - x_turn_follow, y_turn_follow, - x_target, y_target, - 0, data.road_options[0]) + self.odc.initial_road_trajectory( + x_start, + y_start, + x_turn, + y_turn, + x_turn_follow, + y_turn_follow, + x_target, + y_target, + 0, + data.road_options[0], + ) n = len(data.poses) # iterating through global route to create trajectory - for i in range(1, n-1): + for i in range(1, n - 1): # self.loginfo(f"Preplanner going throug global plan {i+1}/{n}") x_target = data.poses[i].position.x y_target = data.poses[i].position.y action = data.road_options[i] - x_target_next = data.poses[i+1].position.x - y_target_next = data.poses[i+1].position.y - self.odc.target_road_trajectory(x_target, y_target, - x_target_next, y_target_next, - action) - - self.odc.target_road_trajectory(data.poses[n-1].position.x, - data.poses[n-1].position.y, - None, None, - data.road_options[n-1]) + x_target_next = data.poses[i + 1].position.x + y_target_next = data.poses[i + 1].position.y + self.odc.target_road_trajectory( + x_target, y_target, x_target_next, y_target_next, action + ) + + self.odc.target_road_trajectory( + data.poses[n - 1].position.x, + data.poses[n - 1].position.y, + None, + None, + data.road_options[n - 1], + ) # trajectory is now stored in the waypoints # waypoints = self.odc.waypoints waypoints = self.odc.remove_outliner(self.odc.waypoints) @@ -170,11 +193,10 @@ def global_route_callback(self, data: CarlaRoute) -> None: stamped_poses = [] for i in range(len(way_x)): position = Point(way_x[i], way_y[i], 0) # way_speed[i]) - quaternion = tf.transformations.quaternion_from_euler(0, - 0, - way_yaw[i]) - orientation = Quaternion(x=quaternion[0], y=quaternion[1], - z=quaternion[2], w=quaternion[3]) + quaternion = tf.transformations.quaternion_from_euler(0, 0, way_yaw[i]) + orientation = Quaternion( + x=quaternion[0], y=quaternion[1], z=quaternion[2], w=quaternion[3] + ) pose = Pose(position, orientation) pos = PoseStamped() pos.header.stamp = rospy.Time.now() @@ -208,8 +230,11 @@ def world_info_callback(self, opendrive: String) -> None: junction_ids = [int(junction.get("id")) for junction in junctions] odc = OpenDriveConverter( - roads=roads, road_ids=road_ids, - junctions=junctions, junction_ids=junction_ids) + roads=roads, + road_ids=road_ids, + junctions=junctions, + junction_ids=junction_ids, + ) odc.convert_roads() odc.convert_junctions() @@ -218,8 +243,9 @@ def world_info_callback(self, opendrive: String) -> None: self.odc = odc if self.global_route_backup is not None: - self.loginfo("PrePlanner: Received a map update retrying " - "route preplanning") + self.loginfo( + "PrePlanner: Received a map update retrying " "route preplanning" + ) self.global_route_callback(self.global_route_backup) def position_callback(self, data: PoseStamped): @@ -232,17 +258,17 @@ def position_callback(self, data: PoseStamped): self.agent_pos = data.pose.position self.agent_ori = data.pose.orientation if self.global_route_backup is not None: - self.loginfo("PrePlanner: Received a pose update retrying " - "route preplanning") + self.loginfo( + "PrePlanner: Received a pose update retrying " "route preplanning" + ) try: self.global_route_callback(self.global_route_backup) except Exception: self.logerr("Preplanner failed -> restart") def dev_load_world_info(self): - file_path = \ - "/workspace/code/planning/src/global_planner/string_world_info.txt" - with open(file_path, 'r') as file: + file_path = "/workspace/code/planning/src/global_planner/string_world_info.txt" + with open(file_path, "r") as file: file_content = file.read() self.logerr("DATA READ") self.world_info_callback(file_content) @@ -260,7 +286,7 @@ def run(self): main function starts the PrePlanner node :param args: """ - roscomp.init('PrePlanner') + roscomp.init("PrePlanner") try: node = PrePlanner() diff --git a/code/planning/src/global_planner/help_functions.py b/code/planning/src/global_planner/help_functions.py index a9f13913..9c419e70 100755 --- a/code/planning/src/global_planner/help_functions.py +++ b/code/planning/src/global_planner/help_functions.py @@ -23,8 +23,7 @@ def euclid_dist(vector1: Tuple[float, float], vector2: Tuple[float, float]): return np.sqrt(sum_sqrt) -def unit_vector(vector: Tuple[float, float], size: float)\ - -> Tuple[float, float]: +def unit_vector(vector: Tuple[float, float], size: float) -> Tuple[float, float]: """ Calculate the unit vector. :param vector: input vector for calculation @@ -35,8 +34,7 @@ def unit_vector(vector: Tuple[float, float], size: float)\ return size * (vector[0] / length), size * (vector[1] / length) -def perpendicular_vector_right(vector: Tuple[float, float])\ - -> Tuple[float, float]: +def perpendicular_vector_right(vector: Tuple[float, float]) -> Tuple[float, float]: """ Perpendicular vector on the right side :param vector: input vector @@ -47,8 +45,7 @@ def perpendicular_vector_right(vector: Tuple[float, float])\ return perp -def perpendicular_vector_left(vector: Tuple[float, float])\ - -> Tuple[float, float]: +def perpendicular_vector_left(vector: Tuple[float, float]) -> Tuple[float, float]: """ Perpendicular vector on the left side :param vector: input vector @@ -59,8 +56,9 @@ def perpendicular_vector_left(vector: Tuple[float, float])\ return perp -def add_vector(v_1: Tuple[float, float], v_2: Tuple[float, float]) \ - -> Tuple[float, float]: +def add_vector( + v_1: Tuple[float, float], v_2: Tuple[float, float] +) -> Tuple[float, float]: """ Addition of two vectors :param v_1: first vector with x and y coordinate @@ -70,8 +68,9 @@ def add_vector(v_1: Tuple[float, float], v_2: Tuple[float, float]) \ return v_1[0] + v_2[0], v_1[1] + v_2[1] -def sub_vector(v_1: Tuple[float, float], v_2: Tuple[float, float]) \ - -> Tuple[float, float]: +def sub_vector( + v_1: Tuple[float, float], v_2: Tuple[float, float] +) -> Tuple[float, float]: """ Subtraction of two vectors :param v_1: first vector with x and y coordinate @@ -81,16 +80,17 @@ def sub_vector(v_1: Tuple[float, float], v_2: Tuple[float, float]) \ return v_1[0] - v_2[0], v_1[1] - v_2[1] -def rotate_vector(vector: Tuple[float, float], angle_rad: float) \ - -> Tuple[float, float]: +def rotate_vector(vector: Tuple[float, float], angle_rad: float) -> Tuple[float, float]: """ Rotate the given vector by an angle with the rotationmatrix :param vector: input vector with x and y coordinate :param angle_rad: rotation angle in rad :return: resulting vector """ - return (cos(angle_rad) * vector[0] - sin(angle_rad) * vector[1], - sin(angle_rad) * vector[0] + cos(angle_rad) * vector[1]) + return ( + cos(angle_rad) * vector[0] - sin(angle_rad) * vector[1], + sin(angle_rad) * vector[0] + cos(angle_rad) * vector[1], + ) def direction_vector(angle_rad: float) -> Tuple[float, float]: @@ -102,8 +102,7 @@ def direction_vector(angle_rad: float) -> Tuple[float, float]: return (cos(angle_rad), sin(angle_rad)) -def scale_vector(vector: Tuple[float, float], new_len: float) \ - -> Tuple[float, float]: +def scale_vector(vector: Tuple[float, float], new_len: float) -> Tuple[float, float]: """ Amplify the length of the given vector :param vector: input vector with x and y coordinate @@ -113,8 +112,7 @@ def scale_vector(vector: Tuple[float, float], new_len: float) \ old_len = vector_len(vector) if old_len == 0: return (0, 0) - scaled_vector = (vector[0] * new_len / old_len, - vector[1] * new_len / old_len) + scaled_vector = (vector[0] * new_len / old_len, vector[1] * new_len / old_len) return scaled_vector @@ -124,11 +122,12 @@ def vector_len(vec: Tuple[float, float]) -> float: :param vec: input vector with x and y coordinate :return: length of the vector """ - return sqrt(vec[0]**2 + vec[1]**2) + return sqrt(vec[0] ** 2 + vec[1] ** 2) -def points_to_vector(p_1: Tuple[float, float], p_2: Tuple[float, float]) \ - -> Tuple[float, float]: +def points_to_vector( + p_1: Tuple[float, float], p_2: Tuple[float, float] +) -> Tuple[float, float]: """ Create the vector starting at p1 and ending at p2 :param p_1: first input vector @@ -138,8 +137,9 @@ def points_to_vector(p_1: Tuple[float, float], p_2: Tuple[float, float]) \ return p_2[0] - p_1[0], p_2[1] - p_1[1] -def end_of_circular_arc(start_point: Tuple[float, float], angle: float, - length: float, radius: float) -> Tuple[float, float]: +def end_of_circular_arc( + start_point: Tuple[float, float], angle: float, length: float, radius: float +) -> Tuple[float, float]: """ Compute the end of a circular arc :param start_point: starting point with x and y coordinate @@ -161,9 +161,9 @@ def end_of_circular_arc(start_point: Tuple[float, float], angle: float, return add_vector(start_point, diff_vec) -def circular_interpolation(start: Tuple[float, float], - end: Tuple[float, float], - arc_radius: float): +def circular_interpolation( + start: Tuple[float, float], end: Tuple[float, float], arc_radius: float +): """ Interpolate points between start / end point on top of the circular arc given by the arc radius @@ -184,11 +184,9 @@ def circular_interpolation(start: Tuple[float, float], # construct the mid-perpendicular of |start, end| to determine the # circle's center conn_middle = ((start[0] + end[0]) / 2, (start[1] + end[1]) / 2) - center_offset = sqrt(pow(arc_radius, 2) - pow(euclid_dist(start, end) / - 2, 2)) - mid_perpend = rotate_vector(points_to_vector(start, end), pi/2 * sign) - circle_center = add_vector(conn_middle, scale_vector(mid_perpend, - center_offset)) + center_offset = sqrt(pow(arc_radius, 2) - pow(euclid_dist(start, end) / 2, 2)) + mid_perpend = rotate_vector(points_to_vector(start, end), pi / 2 * sign) + circle_center = add_vector(conn_middle, scale_vector(mid_perpend, center_offset)) # partition the arc into steps (-> interpol. geometries) arc_circumference = arc_radius * angle @@ -197,15 +195,17 @@ def circular_interpolation(start: Tuple[float, float], # compute the interpolated points on the circle arc vec = points_to_vector(circle_center, start) - rot_angles = [angle * (i / num_steps) for i in range(num_steps+1)] - points = [add_vector(circle_center, rotate_vector(vec, rot * sign)) - for rot in rot_angles] + rot_angles = [angle * (i / num_steps) for i in range(num_steps + 1)] + points = [ + add_vector(circle_center, rotate_vector(vec, rot * sign)) for rot in rot_angles + ] return points -def linear_interpolation(start: Tuple[float, float], end: Tuple[float, float], - interval_m: float): +def linear_interpolation( + start: Tuple[float, float], end: Tuple[float, float], interval_m: float +): """ Interpolate linearly between the given start / end point by putting points according to the interval specified @@ -220,10 +220,14 @@ def linear_interpolation(start: Tuple[float, float], end: Tuple[float, float], steps = max(1, floor(distance / interval_m)) exceeds_interval = distance > interval_m - step_vector = (vector[0] / steps if exceeds_interval else vector[0], - vector[1] / steps if exceeds_interval else vector[1]) - - lin_points = [(start[0] + step_vector[0] * i, - start[1] + step_vector[1] * i) for i in range(steps)] + step_vector = ( + vector[0] / steps if exceeds_interval else vector[0], + vector[1] / steps if exceeds_interval else vector[1], + ) + + lin_points = [ + (start[0] + step_vector[0] * i, start[1] + step_vector[1] * i) + for i in range(steps) + ] lin_points.append(end) return lin_points diff --git a/code/planning/src/global_planner/preplanning_trajectory.py b/code/planning/src/global_planner/preplanning_trajectory.py index c0233918..478addd7 100755 --- a/code/planning/src/global_planner/preplanning_trajectory.py +++ b/code/planning/src/global_planner/preplanning_trajectory.py @@ -37,19 +37,21 @@ class OpenDriveConverter: The OpenDriveConverter needs am OpenDrive Map and the road options from the Carla leaderboard to calculate the global trajectory. """ - def __init__(self, path=None, roads=None, road_ids=None, - junctions=None, junction_ids=None): + + def __init__( + self, path=None, roads=None, road_ids=None, junctions=None, junction_ids=None + ): if roads is None or road_ids is None: - self.roads, self.road_ids = self.list_xodr_properties( - path, name="road") + self.roads, self.road_ids = self.list_xodr_properties(path, name="road") else: self.roads = roads self.road_ids = road_ids if junctions is None or junction_ids is None: self.junctions, self.junction_ids = self.list_xodr_properties( - path, name="junction") + path, name="junction" + ) else: self.junctions = junctions self.junction_ids = junction_ids @@ -90,11 +92,11 @@ def __init__(self, path=None, roads=None, road_ids=None, """ def list_xodr_properties(self, path: str, name: str): - """ Filter properties out of the xodr file - :param path: reference to the xodr file - :param name: name of the property to filter - :return elements: list of the preferred elements - :return element_ids: list of the id values for each element + """Filter properties out of the xodr file + :param path: reference to the xodr file + :param name: name of the property to filter + :return elements: list of the preferred elements + :return element_ids: list of the id values for each element """ # find reference to root node of xodr file root = eTree.parse(path).getroot() @@ -103,7 +105,7 @@ def list_xodr_properties(self, path: str, name: str): return elements, element_ids def convert_roads(self): - """ Filter all road elements in a list. Every road index belongs + """Filter all road elements in a list. Every road index belongs to the same list index. """ max_id = int(max(self.road_ids)) @@ -118,7 +120,7 @@ def convert_roads(self): self.roads = roads_extracted def convert_junctions(self): - """ Filter all junction elements in a list. Every junction index + """Filter all junction elements in a list. Every junction index belongs to the same list index. """ max_id = int(max(self.junction_ids)) @@ -133,7 +135,7 @@ def convert_junctions(self): self.junctions = junctions_extracted def filter_geometry(self): - """ Extract all the geometry information for each road + """Extract all the geometry information for each road use the initialised roads object from the init function this function is only used once when the OpenDrive map is received """ @@ -163,11 +165,11 @@ def filter_geometry(self): else: curvature.append(0.0) geometry_data.append([x, y, heading, curvature, length]) - assert (len(self.roads) == len(geometry_data)) + assert len(self.roads) == len(geometry_data) self.geometry_data = geometry_data def find_current_road(self, x_curr: float, y_curr: float): - """ Extract the current road that fits to the x and y coordinate + """Extract the current road that fits to the x and y coordinate Needed to find the starting road of the agent and for every new waypoint we receive from Carla. :param x_curr: the current global x position of the agent @@ -182,8 +184,9 @@ def find_current_road(self, x_curr: float, y_curr: float): j += 1 continue for i in range(len(road[0])): - diff = help_functions.euclid_dist((road[0][i], road[1][i]), - (x_curr, y_curr)) + diff = help_functions.euclid_dist( + (road[0][i], road[1][i]), (x_curr, y_curr) + ) diff_list.append(diff) diff_index_list.append(j) j += 1 @@ -195,21 +198,26 @@ def find_current_road(self, x_curr: float, y_curr: float): predecessor, successor = self.get_pred_succ(selected_road_id) # Successor and predecessor are a junction -> selected road correct - if self.geometry_data[predecessor] is None and \ - self.geometry_data[successor] is None: + if ( + self.geometry_data[predecessor] is None + and self.geometry_data[successor] is None + ): current_id = selected_road_id # no junction recognized or current road is junction else: - current_id = self.calculate_intervalls_id(agent_position, - selected_road_id, - predecessor, - successor, - junction) + current_id = self.calculate_intervalls_id( + agent_position, selected_road_id, predecessor, successor, junction + ) return current_id - def calculate_intervalls_id(self, agent: Tuple[float, float], - current: int, pred: int, succ: int, - junction: int): + def calculate_intervalls_id( + self, + agent: Tuple[float, float], + current: int, + pred: int, + succ: int, + junction: int, + ): """ The function assumes, that the current chosen road is not the only one that is possible. The current raad is calculated based on all @@ -234,8 +242,7 @@ def calculate_intervalls_id(self, agent: Tuple[float, float], else: id_value = pred # Only one possible other road and current road is not a junction - elif self.geometry_data[pred] is None or self.geometry_data[succ] \ - is None: + elif self.geometry_data[pred] is None or self.geometry_data[succ] is None: if pred is None: road = succ else: @@ -278,9 +285,8 @@ def calculate_intervalls_id(self, agent: Tuple[float, float], id_value = self.get_special_case_id(first, second, agent) return id_value - def get_special_case_id(self, road: int, current: int, - agent: Tuple[float, float]): - """ When the function get_min_dist() returns two solutions with the + def get_special_case_id(self, road: int, current: int, agent: Tuple[float, float]): + """When the function get_min_dist() returns two solutions with the same distance, this function calculated the distance based on the interpolation of the two possible roads. :param road: id value of the successor or predecessor road @@ -292,12 +298,14 @@ def get_special_case_id(self, road: int, current: int, list_r = self.interpolation(road) list_c = self.interpolation(current) - dist_r = [help_functions.euclid_dist( - agent, (list_r[0][i], list_r[1][i])) - for i in range(len(list_r[0]))] - dist_c = [help_functions.euclid_dist( - agent, (list_c[0][i], list_c[1][i])) - for i in range(len(list_r[0]))] + dist_r = [ + help_functions.euclid_dist(agent, (list_r[0][i], list_r[1][i])) + for i in range(len(list_r[0])) + ] + dist_c = [ + help_functions.euclid_dist(agent, (list_c[0][i], list_c[1][i])) + for i in range(len(list_r[0])) + ] value_r = min(dist_r) value_c = min(dist_c) if value_r < value_c: @@ -307,7 +315,7 @@ def get_special_case_id(self, road: int, current: int, return final_id def get_min_dist(self, dist: list): - """ Calculate the minimum distance value from a distance list. + """Calculate the minimum distance value from a distance list. :param dist: list containing all distance values :return: min_dist: the minimum distance/distances from the list """ @@ -323,7 +331,7 @@ def get_min_dist(self, dist: list): return min_dist def get_dist_list(self, pred, current, succ, agent): - """ Calculate the distances between the endpoints of a possible road + """Calculate the distances between the endpoints of a possible road and the current agent position. :param pred: id value of the predecessor road :param current: id value of the assumed road @@ -344,12 +352,20 @@ def get_dist_list(self, pred, current, succ, agent): dist.append(end_d) return dist - def initial_road_trajectory(self, x_curr: float, y_curr: float, - x_target: float, y_target: float, - x_next_t: float, y_next_t: float, - x_first_t: float, y_first_t: float, - yaw: int, command: int): - """ Create the trajectory on the initial road. + def initial_road_trajectory( + self, + x_curr: float, + y_curr: float, + x_target: float, + y_target: float, + x_next_t: float, + y_next_t: float, + x_first_t: float, + y_first_t: float, + yaw: int, + command: int, + ): + """Create the trajectory on the initial road. The agent has to be located on the map. This case has some special requirements. We have to define the driving direction and the trajectory to the next road segment. The start case @@ -369,26 +385,26 @@ def initial_road_trajectory(self, x_curr: float, y_curr: float, junction. :param command: next action from the leaderboard """ - self.road_id = self.find_current_road(x_curr=x_curr, - y_curr=y_curr) + self.road_id = self.find_current_road(x_curr=x_curr, y_curr=y_curr) self.old_id = self.road_id predecessor, successor = self.get_pred_succ(road_id=self.road_id) - self.follow_id, follow_section_id = self.\ - get_initial_next_road_id(predecessor=predecessor, - successor=successor, - x_target=x_target, y_target=y_target, - yaw=yaw) + self.follow_id, follow_section_id = self.get_initial_next_road_id( + predecessor=predecessor, + successor=successor, + x_target=x_target, + y_target=y_target, + yaw=yaw, + ) agent_position = (x_curr, y_curr) # Interpolate the road_id points = self.interpolation(self.road_id) - points = self.check_point_order(points=points, - x_target=x_next_t, - y_target=y_next_t) + points = self.check_point_order( + points=points, x_target=x_next_t, y_target=y_next_t + ) self.reference = copy.deepcopy(points) widths = self.lane_widths(self.road_id) self.width = widths[-1] - self.direction = self.right_or_left(points, x_curr, y_curr, - self.width) + self.direction = self.right_or_left(points, x_curr, y_curr, self.width) points = self.calculate_midpoints(points) # Check if lane change on first road is needed if x_first_t is None and y_first_t is None: @@ -409,11 +425,20 @@ def initial_road_trajectory(self, x_curr: float, y_curr: float, # last points new based on the target lane for the next action if min_dist <= TARGET_DIFF: p = points.copy() - points = self.target_reached(x_target, y_target, - x_next_t, y_next_t, command, - index, self.reference, - follow_section_id, p, - widths, self.direction, True) + points = self.target_reached( + x_target, + y_target, + x_next_t, + y_next_t, + command, + index, + self.reference, + follow_section_id, + p, + widths, + self.direction, + True, + ) # Find and remove the points that are not needed to pass the # first road from the agent position to the end of the road min_dist = float("inf") @@ -436,7 +461,7 @@ def initial_road_trajectory(self, x_curr: float, y_curr: float, self.waypoints = points def calculate_midpoints(self, points: list): - """ Calculate the trajectory points in the middle + """Calculate the trajectory points in the middle of a lane and return the points :param points: list of all trajectory points to reach the following road @@ -453,10 +478,15 @@ def calculate_midpoints(self, points: list): points = self.update_points(points, self.direction, self.width) return points - def target_road_trajectory(self, x_target: float, y_target: float, - x_next_t: float, y_next_t: float, - command: int): - """ Calculate the trajectory to the next waypoint + def target_road_trajectory( + self, + x_target: float, + y_target: float, + x_next_t: float, + y_next_t: float, + command: int, + ): + """Calculate the trajectory to the next waypoint The waypoints are given by the Carla Leaderboard and the next action for the agent :param x_target: x position of the target waypoint @@ -482,17 +512,18 @@ def target_road_trajectory(self, x_target: float, y_target: float, if command <= 3: if x_next_t is not None and y_next_t is not None: predecessor, successor = self.get_pred_succ( - road_id=self.road_id) - follow, follow_section_id = self. \ - get_initial_next_road_id(predecessor=predecessor, - successor=successor, - x_target=x_next_t, - y_target=y_next_t, - yaw=self.pt[2][-1]) + road_id=self.road_id + ) + follow, follow_section_id = self.get_initial_next_road_id( + predecessor=predecessor, + successor=successor, + x_target=x_next_t, + y_target=y_next_t, + yaw=self.pt[2][-1], + ) self.follow_id = self.next_action_id( - x_next_t, y_next_t, - follow_section_id, - self.pt) + x_next_t, y_next_t, follow_section_id, self.pt + ) break # command is lane action else: @@ -505,13 +536,20 @@ def target_road_trajectory(self, x_target: float, y_target: float, min_dist = dist index = i widths = self.lane_widths(self.road_id) - points = self.target_reached(x_target, y_target, - x_next_t, y_next_t, - command, index, - self.reference_l, - self.follow_section, - self.pt, widths, - self.direction, False) + points = self.target_reached( + x_target, + y_target, + x_next_t, + y_next_t, + command, + index, + self.reference_l, + self.follow_section, + self.pt, + widths, + self.direction, + False, + ) points = copy.deepcopy(points) start = len(self.waypoints[0]) - len(points[0]) for i in range(len(points)): @@ -524,19 +562,19 @@ def target_road_trajectory(self, x_target: float, y_target: float, break else: self.road_id = self.follow_id - predecessor, successor = self.get_pred_succ( - road_id=self.road_id) - self.follow_id, follow_section_id = self. \ - get_initial_next_road_id(predecessor=predecessor, - successor=successor, - x_target=x_target, - y_target=y_target, - yaw=self.pt[2][-1]) + predecessor, successor = self.get_pred_succ(road_id=self.road_id) + self.follow_id, follow_section_id = self.get_initial_next_road_id( + predecessor=predecessor, + successor=successor, + x_target=x_target, + y_target=y_target, + yaw=self.pt[2][-1], + ) # Interpolate the road_id points = self.interpolation(self.road_id) - points = self.check_point_order(points=points, - x_target=x_target, - y_target=y_target) + points = self.check_point_order( + points=points, x_target=x_target, y_target=y_target + ) self.reference[0] += copy.deepcopy(points[0]) self.reference[1] += copy.deepcopy(points[1]) self.reference[2] += copy.deepcopy(points[2]) @@ -554,10 +592,15 @@ def target_road_trajectory(self, x_target: float, y_target: float, w_min = None for width in widths: p, v = self.update_one_point( - points[0][0], points[1][0], - points[0][1], points[1][1], - points[0][2], points[1][2], - self.direction, width) + points[0][0], + points[1][0], + points[0][1], + points[1][1], + points[0][2], + points[1][2], + self.direction, + width, + ) diff = help_functions.euclid_dist(p, last_p) if diff < min_diff: min_diff = diff @@ -575,10 +618,9 @@ def target_road_trajectory(self, x_target: float, y_target: float, points = self.calculate_midpoints(points) if command == LEFT or command == RIGHT or command == STRAIGHT: if x_next_t is not None and y_next_t is not None: - self.follow_id = self.next_action_id(x_next_t, - y_next_t, - follow_section_id, - points) + self.follow_id = self.next_action_id( + x_next_t, y_next_t, follow_section_id, points + ) self.pt = points self.reference_l = reference_line self.follow_section = follow_section_id @@ -592,12 +634,20 @@ def target_road_trajectory(self, x_target: float, y_target: float, if dist < min_dist: min_dist = dist index = i - points = self.target_reached(x_target, y_target, - x_next_t, y_next_t, - command, - index, reference_line, - follow_section_id, points, - widths, self.direction, False) + points = self.target_reached( + x_target, + y_target, + x_next_t, + y_next_t, + command, + index, + reference_line, + follow_section_id, + points, + widths, + self.direction, + False, + ) self.add_waypoints(points) self.pt = points self.old_id = self.road_id @@ -608,13 +658,22 @@ def target_road_trajectory(self, x_target: float, y_target: float, self.add_waypoints(points) break - def target_reached(self, target_x: float, target_y: float, - x_next_t: float, y_next_t: float, - command: int, index: int, - reference_line, follow_section_id: int, - points_calc, widths: list, - direction: bool, initial: bool): - """ If a lane change is detected, the trajectory needs to be + def target_reached( + self, + target_x: float, + target_y: float, + x_next_t: float, + y_next_t: float, + command: int, + index: int, + reference_line, + follow_section_id: int, + points_calc, + widths: list, + direction: bool, + initial: bool, + ): + """If a lane change is detected, the trajectory needs to be interpolated again and update the waypoints where the lane change should occur. :param target_x: x coordinate of the target point @@ -631,8 +690,11 @@ def target_reached(self, target_x: float, target_y: float, :param initial: indicates if it is the first interpolated road or not :return: points_calc: new calculated points """ - if command == CHANGE_LEFT or \ - command == CHANGE_RIGHT or command == CHANGE_FOLLOW: + if ( + command == CHANGE_LEFT + or command == CHANGE_RIGHT + or command == CHANGE_FOLLOW + ): if command == CHANGE_LEFT: points = reference_line last_width = self.width @@ -640,12 +702,11 @@ def target_reached(self, target_x: float, target_y: float, ind = widths.index(last_width) if ind == 0: return points_calc - new_width = widths[ind-1] + new_width = widths[ind - 1] self.width = new_width diff = abs(last_width - new_width) steps = int(diff / step_size) - first_widths = [last_width - step_size * i - for i in range(steps)] + first_widths = [last_width - step_size * i for i in range(steps)] for i in range(len(first_widths)): p1_x = points[0][index + i] p1_y = points[1][index + i] @@ -654,14 +715,12 @@ def target_reached(self, target_x: float, target_y: float, if i != len((points[0])) - 2: p3_x = points[0][index + i + 2] p3_y = points[1][index + i + 2] - point, v = self.update_one_point(p1_x, p1_y, - p2_x, p2_y, - p3_x, p3_y, - direction, - first_widths[i]) + point, v = self.update_one_point( + p1_x, p1_y, p2_x, p2_y, p3_x, p3_y, direction, first_widths[i] + ) points_calc[0][index + i] = point[0] points_calc[1][index + i] = point[1] - for i in range(index + len(first_widths), len(points[0])-1): + for i in range(index + len(first_widths), len(points[0]) - 1): p1_x = points[0][i] p1_y = points[1][i] p2_x = points[0][i + 1] @@ -672,18 +731,21 @@ def target_reached(self, target_x: float, target_y: float, else: p3_x = points[0][-1] p3_y = points[1][-1] - point, v = self.update_one_point(p1_x, p1_y, - p2_x, p2_y, - p3_x, p3_y, - direction, - new_width) + point, v = self.update_one_point( + p1_x, p1_y, p2_x, p2_y, p3_x, p3_y, direction, new_width + ) points_calc[0][i] = point[0] points_calc[1][i] = point[1] - point, v = self.update_one_point(p2_x, p2_y, - target_x, target_y, - target_x, target_y, - direction, - new_width) + point, v = self.update_one_point( + p2_x, + p2_y, + target_x, + target_y, + target_x, + target_y, + direction, + new_width, + ) points_calc[0][i + 1] = point[0] points_calc[1][i + 1] = point[1] # change lane right @@ -698,8 +760,7 @@ def target_reached(self, target_x: float, target_y: float, self.width = new_width diff = abs(last_width - new_width) steps = int(diff / step_size) - first_widths = [last_width + step_size * i - for i in range(steps)] + first_widths = [last_width + step_size * i for i in range(steps)] for i in range(len(first_widths)): p1_x = points[0][index + i] p1_y = points[1][index + i] @@ -708,14 +769,12 @@ def target_reached(self, target_x: float, target_y: float, if i != len((points[0])) - 2: p3_x = points[0][index + i + 2] p3_y = points[1][index + i + 2] - point, v = self.update_one_point(p1_x, p1_y, - p2_x, p2_y, - p3_x, p3_y, - direction, - first_widths[i]) + point, v = self.update_one_point( + p1_x, p1_y, p2_x, p2_y, p3_x, p3_y, direction, first_widths[i] + ) points_calc[0][index + i] = point[0] points_calc[1][index + i] = point[1] - for i in range(index + len(first_widths), len(points[0])-1): + for i in range(index + len(first_widths), len(points[0]) - 1): p1_x = points[0][i] p1_y = points[1][i] p2_x = points[0][i + 1] @@ -723,45 +782,48 @@ def target_reached(self, target_x: float, target_y: float, if i != len((points[0])) - 2: p3_x = points[0][i + 2] p3_y = points[1][i + 2] - point, v = self.update_one_point(p1_x, p1_y, - p2_x, p2_y, - p3_x, p3_y, - direction, - new_width) + point, v = self.update_one_point( + p1_x, p1_y, p2_x, p2_y, p3_x, p3_y, direction, new_width + ) points_calc[0][i] = point[0] points_calc[1][i] = point[1] - point, v = self.update_one_point(p2_x, p2_y, - target_x, target_y, - target_x, target_y, - direction, - new_width) + point, v = self.update_one_point( + p2_x, + p2_y, + target_x, + target_y, + target_x, + target_y, + direction, + new_width, + ) points_calc[0][i + 1] = point[0] points_calc[1][i + 1] = point[1] # passing a junction action else: if x_next_t is not None and y_next_t is not None: - predecessor, successor = self.get_pred_succ( - road_id=self.road_id) - follow, follow_section_id = self. \ - get_initial_next_road_id(predecessor=predecessor, - successor=successor, - x_target=x_next_t, - y_target=y_next_t, - yaw=self.pt[2][-1]) - self.follow_id = self.next_action_id(x_next_t, y_next_t, - follow_section_id, - points_calc) + predecessor, successor = self.get_pred_succ(road_id=self.road_id) + follow, follow_section_id = self.get_initial_next_road_id( + predecessor=predecessor, + successor=successor, + x_target=x_next_t, + y_target=y_next_t, + yaw=self.pt[2][-1], + ) + self.follow_id = self.next_action_id( + x_next_t, y_next_t, follow_section_id, points_calc + ) if initial is False: - del points_calc[0][index + 1:] - del points_calc[1][index + 1:] - del points_calc[2][index + 1:] - del points_calc[3][index + 1:] + del points_calc[0][index + 1 :] + del points_calc[1][index + 1 :] + del points_calc[2][index + 1 :] + del points_calc[3][index + 1 :] return points_calc def rad_to_degree(self, radians): - """ Convert radians value to degrees - :param radians: heading value in rad - :return: deg: degree value + """Convert radians value to degrees + :param radians: heading value in rad + :return: deg: degree value """ radians = abs(radians) deg = degrees(radians) @@ -769,9 +831,10 @@ def rad_to_degree(self, radians): deg = 360 - degrees(radians) return deg - def next_action_id(self, x_next_t: float, y_next_t: float, - sec_id: int, points: list): - """ Calculate the next road id for the given action from + def next_action_id( + self, x_next_t: float, y_next_t: float, sec_id: int, points: list + ): + """Calculate the next road id for the given action from the leaderboard :param x_next_t: x coordinate of the next target point :param y_next_t: y coordinate of the next target point @@ -791,22 +854,25 @@ def next_action_id(self, x_next_t: float, y_next_t: float, if current_road is None: junction = sec_id incoming_road = self.road_id - possible_road_ids = self.filter_road_ids(junction, - incoming_road) + possible_road_ids = self.filter_road_ids(junction, incoming_road) last_point_x = points[0][-1] last_point_y = points[1][-1] last_point = (last_point_x, last_point_y) - action_id = self.calculate_action_id(possible_road_ids, - last_point, - x_next_t, y_next_t) + action_id = self.calculate_action_id( + possible_road_ids, last_point, x_next_t, y_next_t + ) else: action_id = self.follow_id return action_id - def calculate_action_id(self, possible_road_ids: list, - last_point: Tuple[float, float], - x_next_t: float, y_next_t: float): - """ Calculate the next road to take from the junction based + def calculate_action_id( + self, + possible_road_ids: list, + last_point: Tuple[float, float], + x_next_t: float, + y_next_t: float, + ): + """Calculate the next road to take from the junction based on the next action from the leaderboard :param possible_road_ids: list of the next possible road ids :param last_point: last calculated point of the trajectory @@ -836,7 +902,7 @@ def calculate_action_id(self, possible_road_ids: list, return possible_road_ids[index] def filter_road_ids(self, junction: int, incoming: int): - """ Filter the road id values of all connecting roads + """Filter the road id values of all connecting roads that are linked to the incoming road :param junction: id value of the junction :param incoming: id value of the incoming road @@ -853,9 +919,9 @@ def filter_road_ids(self, junction: int, incoming: int): return road_ids def lane_widths(self, road_id: int): - """ Filter all lane width values from a given road - :param road_id: the id value of the examined road - :return: widths: list of all width values + """Filter all lane width values from a given road + :param road_id: the id value of the examined road + :return: widths: list of all width values """ road = self.roads[road_id] lanes = road.find("lanes") @@ -902,9 +968,8 @@ def lane_widths(self, road_id: int): widths.append(middle + width * i) return widths - def right_or_left(self, points: list, x_agent: float, y_agent: float, - width: float): - """ Define on which side of the reference line the trajectory + def right_or_left(self, points: list, x_agent: float, y_agent: float, width: float): + """Define on which side of the reference line the trajectory is running. If it returns true the update point function will choose the correct function for the update of the trajectory points @@ -928,14 +993,12 @@ def right_or_left(self, points: list, x_agent: float, y_agent: float, y_follow = points[1][1] point_x = points[0][2] point_y = points[1][2] - point1, v = self.update_one_point(x_start, y_start, - x_follow, y_follow, - point_x, point_y, - True, width) - point2, v = self.update_one_point(x_start, y_start, - x_follow, y_follow, - point_x, point_y, - False, width) + point1, v = self.update_one_point( + x_start, y_start, x_follow, y_follow, point_x, point_y, True, width + ) + point2, v = self.update_one_point( + x_start, y_start, x_follow, y_follow, point_x, point_y, False, width + ) agent = (x_agent, y_agent) dist_1 = help_functions.euclid_dist(point1, agent) dist_2 = help_functions.euclid_dist(point2, agent) @@ -943,11 +1006,18 @@ def right_or_left(self, points: list, x_agent: float, y_agent: float, direction = False return direction - def update_one_point(self, point1_x: float, point1_y: float, - point2_x: float, point2_y: float, - point3_x: float, point3_y: float, - right: bool, width: float): - """ Update the coordinates of a point width the given + def update_one_point( + self, + point1_x: float, + point1_y: float, + point2_x: float, + point2_y: float, + point3_x: float, + point3_y: float, + right: bool, + width: float, + ): + """Update the coordinates of a point width the given width value for the correct lane :param point1_x: x coordinate of the point to update :param point1_y: y coordinate of the point to update @@ -975,7 +1045,7 @@ def update_one_point(self, point1_x: float, point1_y: float, return point, vector def update_points(self, p_list: list, right: bool, width: float): - """ Update the coordinates of a point list width the given + """Update the coordinates of a point list width the given width value for the correct lane :param p_list: list of all trajectory points to reach the following road @@ -999,10 +1069,9 @@ def update_points(self, p_list: list, right: bool, width: float): if i != len((p_list[0])) - 2: point3_x = p_list[0][i + 2] point3_y = p_list[1][i + 2] - point, vector = self.update_one_point(point1_x, point1_y, - point2_x, point2_y, - point3_x, point3_y, - right, width) + point, vector = self.update_one_point( + point1_x, point1_y, point2_x, point2_y, point3_x, point3_y, right, width + ) p_list[0][i] = point[0] p_list[1][i] = point[1] point = (p_list[0][i + 1], p_list[1][i + 1]) @@ -1012,13 +1081,13 @@ def update_points(self, p_list: list, right: bool, width: float): return p_list def add_waypoints(self, points: list): - """ Add calculated points to the trajectory list - :param points: list of all trajectory points - format: [x_points, y_points, heading, speed] - x_points: list of x_values (m) - y_points: list of y_values (m) - heading: list of yaw values (rad) - speed: list of speed limitations (m/s) + """Add calculated points to the trajectory list + :param points: list of all trajectory points + format: [x_points, y_points, heading, speed] + x_points: list of x_values (m) + y_points: list of y_values (m) + heading: list of yaw values (rad) + speed: list of speed limitations (m/s) """ x = copy.deepcopy(points[0]) y = copy.deepcopy(points[1]) @@ -1029,18 +1098,17 @@ def add_waypoints(self, points: list): self.waypoints[2] += hdg self.waypoints[3] += speed - def check_point_order(self, points: list, x_target: float, - y_target: float): - """ Check if the trajectory points have the correct order - :param points: list of all trajectory points - format: [x_points, y_points, heading, speed] - x_points: list of x_values (m) - y_points: list of y_values (m) - heading: list of yaw values (rad) - speed: list of speed limitations (m/s) - :param x_target: x coordinate of the target point - :param y_target: y coordinate of the target point - :return: points: same format as the parameter points + def check_point_order(self, points: list, x_target: float, y_target: float): + """Check if the trajectory points have the correct order + :param points: list of all trajectory points + format: [x_points, y_points, heading, speed] + x_points: list of x_values (m) + y_points: list of y_values (m) + heading: list of yaw values (rad) + speed: list of speed limitations (m/s) + :param x_target: x coordinate of the target point + :param y_target: y coordinate of the target point + :return: points: same format as the parameter points """ target = (x_target, y_target) start_x = points[0][0] @@ -1069,9 +1137,9 @@ def check_point_order(self, points: list, x_target: float, return points def get_speed(self, road_id: int): - """ Filter and calculate the max_speed for the road - :param road_id: id value for the road - :return: speed: speed value for the road in m/s + """Filter and calculate the max_speed for the road + :param road_id: id value for the road + :return: speed: speed value for the road in m/s """ road = self.roads[road_id] speed_type = road.find("type").find("speed").get("unit") @@ -1087,15 +1155,15 @@ def get_speed(self, road_id: int): return speed def interpolation(self, road_id: int): - """ Interpolate over a complete road - :param road_id: id value of the current road - :return: waypoints: list of all trajectory points to reach - the following road - format: [x_points, y_points, heading, speed] - x_points: list of x_values (m) - y_points: list of y_values (m) - heading: list of yaw values (rad) - speed: list of speed limitations (m/s) + """Interpolate over a complete road + :param road_id: id value of the current road + :return: waypoints: list of all trajectory points to reach + the following road + format: [x_points, y_points, heading, speed] + x_points: list of x_values (m) + y_points: list of y_values (m) + heading: list of yaw values (rad) + speed: list of speed limitations (m/s) """ x = list() y = list() @@ -1114,18 +1182,16 @@ def interpolation(self, road_id: int): yd = length * sin(hdg) end = help_functions.add_vector(start, (xd, yd)) points = help_functions.linear_interpolation( - start=start, - end=end, - interval_m=INTERVALL) + start=start, end=end, interval_m=INTERVALL + ) else: radius = self.geometry_data[road_id][3][i] end = help_functions.end_of_circular_arc( - start_point=start, angle=hdg, - length=length, radius=radius) + start_point=start, angle=hdg, length=length, radius=radius + ) points = help_functions.circular_interpolation( - start=start, - end=end, - arc_radius=radius) + start=start, end=end, arc_radius=radius + ) for j in range(len(points)): x.append(points[j][0]) y.append(points[j][1]) @@ -1137,7 +1203,7 @@ def interpolation(self, road_id: int): return points def remove_outliner(self, points): - """ Function checks if distance between two following trajectory + """Function checks if distance between two following trajectory points is to small or to big. Delete and update waypoints if necessary. :param points: points: list of all trajectory points format: [x_points, y_points, heading, speed] @@ -1154,10 +1220,10 @@ def remove_outliner(self, points): dist = help_functions.euclid_dist(p, p_next) # point is to close to the following point (0.5m) if dist < 0.5: - delete_index.append(i+1) + delete_index.append(i + 1) # outliner point elif dist > 3: - delete_index.append(i+1) + delete_index.append(i + 1) # delete the points with the calculated indices number = 0 for i in delete_index: @@ -1170,11 +1236,11 @@ def remove_outliner(self, points): return points def get_endpoints(self, road_id: int): - """ Calculate the startpoint and endpoint of a given road - :param road_id: the road id of the examined road - :return: start_point, end_point - start_point: x and y coordinate of the starting point - end_point: x and y coordinate of the ending point + """Calculate the startpoint and endpoint of a given road + :param road_id: the road id of the examined road + :return: start_point, end_point + start_point: x and y coordinate of the starting point + end_point: x and y coordinate of the ending point """ size = len(self.geometry_data[road_id][0]) x_start = self.geometry_data[road_id][0][0] @@ -1190,7 +1256,7 @@ def get_endpoints(self, road_id: int): last_start = (x, y) # check the last curvature value to see if it is line or arc - if self.geometry_data[road_id][3][size-1] == LINE: + if self.geometry_data[road_id][3][size - 1] == LINE: xd = length * cos(hdg) yd = length * sin(hdg) # subtract a small value due to inaccuracy @@ -1204,17 +1270,21 @@ def get_endpoints(self, road_id: int): yd += 0.05 end_point = help_functions.add_vector(last_start, (xd, yd)) else: - radius = self.geometry_data[road_id][3][size-1] + radius = self.geometry_data[road_id][3][size - 1] end_point = help_functions.end_of_circular_arc( - start_point=last_start, angle=hdg, - length=length, radius=radius) + start_point=last_start, angle=hdg, length=length, radius=radius + ) return start_point, end_point - def get_initial_next_road_id(self, predecessor: int, - successor: int, - x_target: float, y_target: float, - yaw: int): - """ Find the next road to drive + def get_initial_next_road_id( + self, + predecessor: int, + successor: int, + x_target: float, + y_target: float, + yaw: int, + ): + """Find the next road to drive When the agent starts driving it is not sure if he has to follow his successor or his predecessor. This function calculates the next road id, based on the dist to the target point. The road, who is nearer to @@ -1239,16 +1309,14 @@ def get_initial_next_road_id(self, predecessor: int, final_id = predecessor else: min_distances = list() - x_road_p, y_road_p, pred = self.\ - get_next_road_point(predecessor, yaw) + x_road_p, y_road_p, pred = self.get_next_road_point(predecessor, yaw) point1, point2 = self.get_endpoints(pred) dist1 = help_functions.euclid_dist(point1, target) min_distances.append(dist1) dist2 = help_functions.euclid_dist(point2, target) min_distances.append(dist2) - x_road_s, y_road_s, succ = self.\ - get_next_road_point(successor, yaw) + x_road_s, y_road_s, succ = self.get_next_road_point(successor, yaw) point3, point4 = self.get_endpoints(succ) dist3 = help_functions.euclid_dist(point3, target) min_distances.append(dist3) @@ -1266,7 +1334,7 @@ def get_initial_next_road_id(self, predecessor: int, return final_id, section_id def get_pred_succ(self, road_id: int): - """ Find the predecessor and the successor road of the current road + """Find the predecessor and the successor road of the current road If there is only a successor or only a predecessor, this function handles these cases :param road_id: id of the current road @@ -1279,7 +1347,7 @@ def get_pred_succ(self, road_id: int): curr_road = self.roads[road_id] link = curr_road.find("link") # Road needs a successor or predecessor - assert (len(link) > 0) + assert len(link) > 0 # if only one following road if len(link) == 1: next_road_id = link[0].get("elementId") @@ -1293,20 +1361,20 @@ def get_pred_succ(self, road_id: int): # predecessor and successor -> only choose which direction # to drive else: - predecessor = int(curr_road.find("link").find("predecessor"). - get("elementId")) - successor = int(curr_road.find("link").find("successor"). - get("elementId")) + predecessor = int( + curr_road.find("link").find("predecessor").get("elementId") + ) + successor = int(curr_road.find("link").find("successor").get("elementId")) return predecessor, successor def get_next_road_point(self, road_id: int, yaw: int): - """ The function returns the x and y coordinate for a given road - :param road_id: the id value of the preferred road - :param yaw: yaw value of the agent - :return: x, y, road_id - x: value of the x coordinate - y: value of the y coordinate - road_id: id of the chosen road + """The function returns the x and y coordinate for a given road + :param road_id: the id value of the preferred road + :param yaw: yaw value of the agent + :return: x, y, road_id + x: value of the x coordinate + y: value of the y coordinate + road_id: id of the chosen road """ line_list = list() # check if it is a junction diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 0c013364..74b681b2 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -3,7 +3,7 @@ from ros_compatibility.node import CompatibleNode from rospy import Subscriber, Publisher from geometry_msgs.msg import PoseStamped -from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo +from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo from nav_msgs.msg import Path from std_msgs.msg import Float32MultiArray, Float32, Bool import numpy as np @@ -16,7 +16,7 @@ class ACC(CompatibleNode): """ def __init__(self): - super(ACC, self).__init__('ACC') + super(ACC, self).__init__("ACC") self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) @@ -25,63 +25,67 @@ def __init__(self): Bool, f"/paf/{self.role_name}/unstuck_flag", self.__get_unstuck_flag, - qos_profile=1) + qos_profile=1, + ) self.unstuck_distance_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/unstuck_distance", self.__get_unstuck_distance, - qos_profile=1) + qos_profile=1, + ) # Get current speed self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__get_current_velocity, - qos_profile=1) + qos_profile=1, + ) # Get initial set of speed limits from global planner self.speed_limit_OD_sub: Subscriber = self.new_subscription( Float32MultiArray, f"/paf/{self.role_name}/speed_limits_OpenDrive", self.__set_speed_limits_opendrive, - qos_profile=1) + qos_profile=1, + ) # Get trajectory to determine current speed limit self.trajectory_sub: Subscriber = self.new_subscription( Path, f"/paf/{self.role_name}/trajectory_global", self.__set_trajectory, - qos_profile=1) + qos_profile=1, + ) # Get current position to determine current waypoint self.pose_sub: Subscriber = self.new_subscription( msg_type=PoseStamped, topic="/paf/" + self.role_name + "/current_pos", callback=self.__current_position_callback, - qos_profile=1) + qos_profile=1, + ) # Get approximated speed from obstacle in front self.approx_speed_sub = self.new_subscription( Float32MultiArray, f"/paf/{self.role_name}/collision", self.__collision_callback, - qos_profile=1) + qos_profile=1, + ) # Publish desired speed to acting self.velocity_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/acc_velocity", - qos_profile=1) + Float32, f"/paf/{self.role_name}/acc_velocity", qos_profile=1 + ) # Publish current waypoint and speed limit self.wp_publisher: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/current_wp", - qos_profile=1) + Float32, f"/paf/{self.role_name}/current_wp", qos_profile=1 + ) self.speed_limit_publisher: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/speed_limit", - qos_profile=1) + Float32, f"/paf/{self.role_name}/speed_limit", qos_profile=1 + ) # unstuck attributes self.__unstuck_flag: bool = False @@ -172,11 +176,9 @@ def __current_position_callback(self, data: PoseStamped): agent = data.pose.position # Get current waypoint - current_wp = self.__trajectory.poses[self.__current_wp_index].\ - pose.position + current_wp = self.__trajectory.poses[self.__current_wp_index].pose.position # Get next waypoint - next_wp = self.__trajectory.poses[self.__current_wp_index + 1].\ - pose.position + next_wp = self.__trajectory.poses[self.__current_wp_index + 1].pose.position # distances from agent to current and next waypoint d_old = abs(agent.x - current_wp.x) + abs(agent.y - current_wp.y) d_new = abs(agent.x - next_wp.x) + abs(agent.y - next_wp.y) @@ -185,19 +187,16 @@ def __current_position_callback(self, data: PoseStamped): # update current waypoint and corresponding speed limit self.__current_wp_index += 1 self.wp_publisher.publish(self.__current_wp_index) - self.speed_limit = \ - self.__speed_limits_OD[self.__current_wp_index] + self.speed_limit = self.__speed_limits_OD[self.__current_wp_index] self.speed_limit_publisher.publish(self.speed_limit) # in case we used the unstuck routine to drive backwards # we have to follow WPs that are already passed elif self.__unstuck_flag: - if self.__unstuck_distance is None\ - or self.__unstuck_distance == -1: + if self.__unstuck_distance is None or self.__unstuck_distance == -1: return self.__current_wp_index -= int(self.__unstuck_distance) self.wp_publisher.publish(self.__current_wp_index) - self.speed_limit = \ - self.__speed_limits_OD[self.__current_wp_index] + self.speed_limit = self.__speed_limits_OD[self.__current_wp_index] self.speed_limit_publisher.publish(self.speed_limit) def run(self): @@ -205,27 +204,31 @@ def run(self): Control loop :return: """ + def loop(timer_event=None): """ Permanent checks if distance to a possible object is too small and publishes the desired speed to motion planning """ - if self.obstacle_distance is not None and \ - self.obstacle_speed is not None and \ - self.__current_velocity is not None: + if ( + self.obstacle_distance is not None + and self.obstacle_speed is not None + and self.__current_velocity is not None + ): # If we have obstalce information, # we can calculate the safe speed safety_distance = calculate_rule_of_thumb( - False, self.__current_velocity) + False, self.__current_velocity + ) if self.obstacle_distance < safety_distance: # If safety distance is reached, we want to reduce the # speed to meet the desired distance # https://encyclopediaofmath.org/index.php?title=Linear_interpolation - safe_speed = self.obstacle_speed * \ - (self.obstacle_distance / safety_distance) + safe_speed = self.obstacle_speed * ( + self.obstacle_distance / safety_distance + ) # Interpolate speed for smoother braking - safe_speed = interpolate_speed(safe_speed, - self.__current_velocity) + safe_speed = interpolate_speed(safe_speed, self.__current_velocity) if safe_speed < 1.0: safe_speed = 0 self.velocity_pub.publish(safe_speed) @@ -255,7 +258,7 @@ def loop(timer_event=None): main function starts the ACC node :param args: """ - roscomp.init('ACC') + roscomp.init("ACC") try: node = ACC() diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index f9550717..70f0946a 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -2,12 +2,13 @@ # import rospy import numpy as np import rospy + # import tf.transformations import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode from rospy import Subscriber -from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo +from carla_msgs.msg import CarlaSpeedometer # , CarlaWorldInfo from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool @@ -21,7 +22,7 @@ class CollisionCheck(CompatibleNode): """ def __init__(self): - super(CollisionCheck, self).__init__('CollisionCheck') + super(CollisionCheck, self).__init__("CollisionCheck") self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) # Subscriber for current speed @@ -29,28 +30,27 @@ def __init__(self): CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__get_current_velocity, - qos_profile=1) + qos_profile=1, + ) # Subscriber for lidar objects self.lidar_dist = self.new_subscription( Float32MultiArray, f"/paf/{self.role_name}/Center/object_distance", self.__set_all_distances, - qos_profile=1) + qos_profile=1, + ) # Publisher for emergency stop self.emergency_pub = self.new_publisher( - Bool, - f"/paf/{self.role_name}/emergency", - qos_profile=1) + Bool, f"/paf/{self.role_name}/emergency", qos_profile=1 + ) # Publisher for distance to collision self.collision_pub = self.new_publisher( - Float32MultiArray, - f"/paf/{self.role_name}/collision", - qos_profile=1) + Float32MultiArray, f"/paf/{self.role_name}/collision", qos_profile=1 + ) # Publisher for distance to oncoming traffic self.oncoming_pub = self.new_publisher( - Float32, - f"/paf/{self.role_name}/oncoming", - qos_profile=1) + Float32, f"/paf/{self.role_name}/oncoming", qos_profile=1 + ) # Variables to save vehicle data self.__current_velocity: float = None self.__object_first_position: tuple = None @@ -103,10 +103,11 @@ def __set_distance(self, data: Float32MultiArray): """ # Filter onjects in front nearest_object = filter_vision_objects(data.data, False) - if nearest_object is None and \ - self.__object_last_position is not None and \ - rospy.get_rostime() - self.__object_last_position[0] > \ - rospy.Duration(2): + if ( + nearest_object is None + and self.__object_last_position is not None + and rospy.get_rostime() - self.__object_last_position[0] > rospy.Duration(2) + ): # If no object is in front and last object is older than 2 seconds # we assume no object is in front self.update_distance(True) @@ -129,10 +130,12 @@ def __set_distance_oncoming(self, data: Float32MultiArray): """ # Filter for oncoming traffic objects nearest_object = filter_vision_objects(data.data, True) - if (nearest_object is None and - self.__last_position_oncoming is not None and - rospy.get_rostime() - self.__last_position_oncoming[0] > - rospy.Duration(2)): + if ( + nearest_object is None + and self.__last_position_oncoming is not None + and rospy.get_rostime() - self.__last_position_oncoming[0] + > rospy.Duration(2) + ): # If no oncoming traffic found and last object is older than 2 # seconds we assume no object is in front self.update_distance_oncoming(True) @@ -141,8 +144,7 @@ def __set_distance_oncoming(self, data: Float32MultiArray): # If no oncoming traffic abort return - self.__last_position_oncoming = \ - (rospy.get_rostime(), nearest_object[1]) + self.__last_position_oncoming = (rospy.get_rostime(), nearest_object[1]) # Update oncoming traffic distance if this is first object since reset self.update_distance_oncoming(False) # Publish oncoming traffic to Decision Making @@ -170,26 +172,28 @@ def update_distance_oncoming(self, reset): def calculate_obstacle_speed(self): """Caluclate the speed of the obstacle in front of the ego vehicle - based on the distance between to timestamps. - Then check for collision + based on the distance between to timestamps. + Then check for collision """ # Check if current speed from vehicle is not None - if self.__current_velocity is None or \ - self.__object_first_position is None or \ - self.__object_last_position is None: + if ( + self.__current_velocity is None + or self.__object_first_position is None + or self.__object_last_position is None + ): return # Calculate time since last position update - rospy_time_difference = self.__object_last_position[0] - \ - self.__object_first_position[0] + rospy_time_difference = ( + self.__object_last_position[0] - self.__object_first_position[0] + ) # Use nanoseconds for time difference to be more accurate # and reduce error - time_difference = rospy_time_difference.nsecs/1e9 + time_difference = rospy_time_difference.nsecs / 1e9 # Calculate distance (in m) - distance = self.__object_last_position[1] - \ - self.__object_first_position[1] + distance = self.__object_last_position[1] - self.__object_first_position[1] try: # Speed difference is distance/time (m/s) - relative_speed = distance/time_difference + relative_speed = distance / time_difference except ZeroDivisionError: # If time difference is 0, we cannot calculate speed return @@ -204,7 +208,10 @@ def calculate_obstacle_speed(self): # Update first position to calculate speed when next object is detected self.__object_first_position = self.__object_last_position - def __get_current_velocity(self, data: CarlaSpeedometer,): + def __get_current_velocity( + self, + data: CarlaSpeedometer, + ): """Saves current velocity of the ego vehicle Args: @@ -228,7 +235,7 @@ def time_to_collision(self, obstacle_speed, distance): return distance / (self.__current_velocity - obstacle_speed) def check_crash(self, obstacle): - """ Checks if and when the ego vehicle will crash + """Checks if and when the ego vehicle will crash with the obstacle in front Args: @@ -239,8 +246,7 @@ def check_crash(self, obstacle): collision_time = self.time_to_collision(obstacle_speed, distance) # Calculate emergency distance based on current speed - emergency_distance = calculate_rule_of_thumb( - True, self.__current_velocity) + emergency_distance = calculate_rule_of_thumb(True, self.__current_velocity) if collision_time > 0: # If time to collision is positive, a collision is ahead if distance < emergency_distance: @@ -268,7 +274,7 @@ def run(self): main function starts the CollisionCheck node :param args: """ - roscomp.init('CollisionCheck') + roscomp.init("CollisionCheck") try: node = CollisionCheck() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index d7448df9..61c3a4e3 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -17,8 +17,7 @@ import planning # noqa: F401 from behavior_agent.behaviours import behavior_speed as bs -from utils import convert_to_ms, spawn_car, NUM_WAYPOINTS, \ - TARGET_DISTANCE_TO_STOP +from utils import convert_to_ms, spawn_car, NUM_WAYPOINTS, TARGET_DISTANCE_TO_STOP # from scipy.spatial._kdtree import KDTree @@ -35,7 +34,7 @@ class MotionPlanning(CompatibleNode): """ def __init__(self): - super(MotionPlanning, self).__init__('MotionPlanning') + super(MotionPlanning, self).__init__("MotionPlanning") self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 0.05) @@ -65,101 +64,107 @@ def __init__(self): self.init_overtake_pos = None # Subscriber self.test_sub = self.new_subscription( - Float32, - f"/paf/{self.role_name}/spawn_car", - spawn_car, - qos_profile=1) + Float32, f"/paf/{self.role_name}/spawn_car", spawn_car, qos_profile=1 + ) self.speed_limit_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/speed_limit", self.__set_speed_limit, - qos_profile=1) + qos_profile=1, + ) self.velocity_sub: Subscriber = self.new_subscription( CarlaSpeedometer, f"/carla/{self.role_name}/Speed", self.__get_current_velocity, - qos_profile=1) + qos_profile=1, + ) self.head_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/current_heading", self.__set_heading, - qos_profile=1) + qos_profile=1, + ) self.trajectory_sub = self.new_subscription( Path, f"/paf/{self.role_name}/trajectory_global", self.__set_trajectory, - qos_profile=1) + qos_profile=1, + ) self.current_pos_sub = self.new_subscription( PoseStamped, f"/paf/{self.role_name}/current_pos", self.__set_current_pos, - qos_profile=1) + qos_profile=1, + ) self.curr_behavior_sub: Subscriber = self.new_subscription( String, f"/paf/{self.role_name}/curr_behavior", self.__set_curr_behavior, - qos_profile=1) + qos_profile=1, + ) self.emergency_sub: Subscriber = self.new_subscription( Bool, f"/paf/{self.role_name}/unchecked_emergency", self.__check_emergency, - qos_profile=1) + qos_profile=1, + ) self.acc_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/acc_velocity", self.__set_acc_speed, - qos_profile=1) + qos_profile=1, + ) self.stopline_sub: Subscriber = self.new_subscription( Waypoint, f"/paf/{self.role_name}/waypoint_distance", self.__set_stopline, - qos_profile=1) + qos_profile=1, + ) self.change_point_sub: Subscriber = self.new_subscription( LaneChange, f"/paf/{self.role_name}/lane_change_distance", self.__set_change_point, - qos_profile=1) + qos_profile=1, + ) self.coll_point_sub: Subscriber = self.new_subscription( Float32MultiArray, f"/paf/{self.role_name}/collision", self.__set_collision_point, - qos_profile=1) + qos_profile=1, + ) self.traffic_y_sub: Subscriber = self.new_subscription( Int16, f"/paf/{self.role_name}/Center/traffic_light_y_distance", self.__set_traffic_y_distance, - qos_profile=1) + qos_profile=1, + ) self.unstuck_distance_sub: Subscriber = self.new_subscription( Float32, f"/paf/{self.role_name}/unstuck_distance", self.__set_unstuck_distance, - qos_profile=1) + qos_profile=1, + ) # Publisher self.traj_pub: Publisher = self.new_publisher( - msg_type=Path, - topic=f"/paf/{self.role_name}/trajectory", - qos_profile=1) + msg_type=Path, topic=f"/paf/{self.role_name}/trajectory", qos_profile=1 + ) self.velocity_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/target_velocity", - qos_profile=1) + Float32, f"/paf/{self.role_name}/target_velocity", qos_profile=1 + ) self.wp_subs = self.new_subscription( - Float32, - f"/paf/{self.role_name}/current_wp", - self.__set_wp, - qos_profile=1) + Float32, f"/paf/{self.role_name}/current_wp", self.__set_wp, qos_profile=1 + ) self.overtake_success_pub = self.new_publisher( - Float32, - f"/paf/{self.role_name}/overtake_success", - qos_profile=1) + Float32, f"/paf/{self.role_name}/overtake_success", qos_profile=1 + ) self.logdebug("MotionPlanning started") self.counter = 0 @@ -209,9 +214,9 @@ def __set_current_pos(self, data: PoseStamped): Args: data (PoseStamped): current position """ - self.current_pos = np.array([data.pose.position.x, - data.pose.position.y, - data.pose.position.z]) + self.current_pos = np.array( + [data.pose.position.x, data.pose.position.y, data.pose.position.z] + ) def __set_traffic_y_distance(self, data): if data is not None: @@ -237,20 +242,25 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): normal_x_offset = 2 unstuck_x_offset = 3 # could need adjustment with better steering if unstuck: - selection = pose_list[int(currentwp)-2:int(currentwp) + - int(distance)+2 + NUM_WAYPOINTS] + selection = pose_list[ + int(currentwp) - 2 : int(currentwp) + int(distance) + 2 + NUM_WAYPOINTS + ] else: - selection = pose_list[int(currentwp) + int(distance/2): - int(currentwp) + - int(distance) + NUM_WAYPOINTS] + selection = pose_list[ + int(currentwp) + + int(distance / 2) : int(currentwp) + + int(distance) + + NUM_WAYPOINTS + ] waypoints = self.convert_pose_to_array(selection) if unstuck is True: offset = np.array([unstuck_x_offset, 0, 0]) else: offset = np.array([normal_x_offset, 0, 0]) - rotation_adjusted = Rotation.from_euler('z', self.current_heading + - math.radians(90)) + rotation_adjusted = Rotation.from_euler( + "z", self.current_heading + math.radians(90) + ) offset_front = rotation_adjusted.apply(offset) offset_front = offset_front[:2] waypoints_off = waypoints + offset_front @@ -261,8 +271,7 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): result = [] for i in range(len(result_x)): position = Point(result_x[i], result_y[i], 0) - orientation = Quaternion(x=0, y=0, - z=0, w=0) + orientation = Quaternion(x=0, y=0, z=0, w=0) pose = Pose(position, orientation) pos = PoseStamped() pos.header.frame_id = "global" @@ -272,12 +281,17 @@ def overtake_fallback(self, distance, pose_list, unstuck=False): path.header.stamp = rospy.Time.now() path.header.frame_id = "global" if unstuck: - path.poses = pose_list[:int(currentwp)-2] + \ - result + pose_list[int(currentwp) + - int(distance) + 2 + NUM_WAYPOINTS:] + path.poses = ( + pose_list[: int(currentwp) - 2] + + result + + pose_list[int(currentwp) + int(distance) + 2 + NUM_WAYPOINTS :] + ) else: - path.poses = pose_list[:int(currentwp) + int(distance/2)] + \ - result + pose_list[int(currentwp + distance + NUM_WAYPOINTS):] + path.poses = ( + pose_list[: int(currentwp) + int(distance / 2)] + + result + + pose_list[int(currentwp + distance + NUM_WAYPOINTS) :] + ) self.trajectory = path @@ -389,8 +403,9 @@ def convert_pose_to_array(poses: np.array): """ result_array = np.empty((len(poses), 2)) for pose in range(len(poses)): - result_array[pose] = np.array([poses[pose].pose.position.x, - poses[pose].pose.position.y]) + result_array[pose] = np.array( + [poses[pose].pose.position.x, poses[pose].pose.position.y] + ) return result_array def __check_emergency(self, data: Bool): @@ -435,8 +450,7 @@ def __set_stopline(self, data: Waypoint) -> float: def __set_change_point(self, data: LaneChange): if data is not None: - self.__change_point = \ - (data.distance, data.isLaneChange, data.roadOption) + self.__change_point = (data.distance, data.isLaneChange, data.roadOption) def __set_collision_point(self, data: Float32MultiArray): if data.data is not None: @@ -475,10 +489,10 @@ def __get_speed_unstuck(self, behavior: str) -> float: self.logfatal("Unstuck distance not set") return speed - if self.init_overtake_pos is not None \ - and self.current_pos is not None: + if self.init_overtake_pos is not None and self.current_pos is not None: distance = np.linalg.norm( - self.init_overtake_pos[:2] - self.current_pos[:2]) + self.init_overtake_pos[:2] - self.current_pos[:2] + ) # self.logfatal(f"Unstuck Distance in mp: {distance}") # clear distance to last unstuck -> avoid spamming overtake if distance > UNSTUCK_OVERTAKE_FLAG_CLEAR_DISTANCE: @@ -490,8 +504,7 @@ def __get_speed_unstuck(self, behavior: str) -> float: # create overtake trajectory starting 6 meteres before # the obstacle # 6 worked well in tests, but can be adjusted - self.overtake_fallback(self.unstuck_distance, pose_list, - unstuck=True) + self.overtake_fallback(self.unstuck_distance, pose_list, unstuck=True) self.logfatal("Overtake Trajectory while unstuck!") self.unstuck_overtake_flag = True self.init_overtake_pos = self.current_pos[:2] @@ -550,7 +563,7 @@ def __get_speed_overtake(self, behavior: str) -> float: elif behavior == bs.ot_enter_slow.name: speed = self.__calc_speed_to_stop_overtake() elif behavior == bs.ot_leave.name: - speed = convert_to_ms(30.) + speed = convert_to_ms(30.0) return speed def __get_speed_cruise(self) -> float: @@ -561,8 +574,7 @@ def __calc_speed_to_stop_intersection(self) -> float: stopline = self.__calc_virtual_stopline() # calculate speed needed for stopping - v_stop = max(convert_to_ms(10.), - convert_to_ms(stopline / 0.8)) + v_stop = max(convert_to_ms(10.0), convert_to_ms(stopline / 0.8)) if v_stop > bs.int_app_init.speed: v_stop = bs.int_app_init.speed if stopline < target_distance: @@ -572,8 +584,7 @@ def __calc_speed_to_stop_intersection(self) -> float: def __calc_speed_to_stop_lanechange(self) -> float: stopline = self.__calc_virtual_change_point() - v_stop = max(convert_to_ms(10.), - convert_to_ms(stopline / 0.8)) + v_stop = max(convert_to_ms(10.0), convert_to_ms(stopline / 0.8)) if v_stop > bs.lc_app_init.speed: v_stop = bs.lc_app_init.speed if stopline < TARGET_DISTANCE_TO_STOP: @@ -582,8 +593,7 @@ def __calc_speed_to_stop_lanechange(self) -> float: def __calc_speed_to_stop_overtake(self) -> float: stopline = self.__calc_virtual_overtake() - v_stop = max(convert_to_ms(10.), - convert_to_ms(stopline / 0.8)) + v_stop = max(convert_to_ms(10.0), convert_to_ms(stopline / 0.8)) if stopline < TARGET_DISTANCE_TO_STOP: v_stop = 0.0 @@ -608,8 +618,7 @@ def __calc_virtual_stopline(self) -> float: return 0.0 def __calc_virtual_overtake(self) -> float: - if (self.__collision_point is not None) and \ - self.__collision_point != np.inf: + if (self.__collision_point is not None) and self.__collision_point != np.inf: return self.__collision_point else: return 0.0 @@ -621,15 +630,17 @@ def run(self): """ def loop(timer_event=None): - if (self.__curr_behavior is not None and - self.__acc_speed is not None and - self.__corners is not None): + if ( + self.__curr_behavior is not None + and self.__acc_speed is not None + and self.__corners is not None + ): self.trajectory.header.stamp = rospy.Time.now() self.traj_pub.publish(self.trajectory) - self.update_target_speed(self.__acc_speed, - self.__curr_behavior) + self.update_target_speed(self.__acc_speed, self.__curr_behavior) else: self.velocity_pub.publish(0.0) + self.new_timer(self.control_loop_rate, loop) self.spin() @@ -639,7 +650,7 @@ def loop(timer_event=None): main function starts the MotionPlanning node :param args: """ - roscomp.init('MotionPlanning') + roscomp.init("MotionPlanning") try: node = MotionPlanning() node.run() diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 63cf5600..d976506d 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -3,6 +3,7 @@ import math import carla import os + # import rospy @@ -56,17 +57,19 @@ def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): scale = math.cos(lat_ref * math.pi / 180.0) mx = scale * lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0 - my = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + lat_ref) * - math.pi / 360.0)) + my = ( + scale + * EARTH_RADIUS_EQUA + * math.log(math.tan((90.0 + lat_ref) * math.pi / 360.0)) + ) mx += x my -= y lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale) - lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) /\ - math.pi - 90.0 + lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) / math.pi - 90.0 z = 703 - return {'lat': lat, 'lon': lon, 'z': z} + return {"lat": lat, "lon": lon, "z": z} def calculate_rule_of_thumb(emergency, speed): @@ -81,7 +84,7 @@ def calculate_rule_of_thumb(emergency, speed): float: distance calculated with rule of thumb """ reaction_distance = speed - braking_distance = (speed * 0.36)**2 + braking_distance = (speed * 0.36) ** 2 if emergency: # Emergency brake is really effective in Carla return reaction_distance + braking_distance / 2 @@ -89,8 +92,9 @@ def calculate_rule_of_thumb(emergency, speed): return reaction_distance + braking_distance -def approx_obstacle_pos(distance: float, heading: float, - ego_pos: np.array, speed: float): +def approx_obstacle_pos( + distance: float, heading: float, ego_pos: np.array, speed: float +): """calculate the position of the obstacle in the global coordinate system based on ego position, heading and distance @@ -103,7 +107,7 @@ def approx_obstacle_pos(distance: float, heading: float, Returns: np.array: approximated position of the obstacle """ - rotation_matrix = Rotation.from_euler('z', heading) + rotation_matrix = Rotation.from_euler("z", heading) # Create distance vector with 0 rotation relative_position_local = np.array([distance, 0, 0]) @@ -120,18 +124,18 @@ def approx_obstacle_pos(distance: float, heading: float, # calculate the front left corner of the vehicle offset = np.array([1, 0, 0]) - rotation_adjusted = Rotation.from_euler('z', heading + math.radians(90)) + rotation_adjusted = Rotation.from_euler("z", heading + math.radians(90)) offset_front = rotation_adjusted.apply(offset) # calculate back right corner of the vehicle - rotation_adjusted = Rotation.from_euler('z', heading + math.radians(270)) + rotation_adjusted = Rotation.from_euler("z", heading + math.radians(270)) offset_back = rotation_adjusted.apply(offset) - vehicle_position_global_end = vehicle_position_global_start + \ - length_vector + offset_back + vehicle_position_global_end = ( + vehicle_position_global_start + length_vector + offset_back + ) - return vehicle_position_global_start + offset_front, \ - vehicle_position_global_end + return vehicle_position_global_start + offset_front, vehicle_position_global_end def convert_to_ms(speed: float): @@ -152,8 +156,8 @@ def spawn_car(distance): Args: distance (float): distance """ - CARLA_HOST = os.environ.get('CARLA_HOST', 'paf-carla-simulator-1') - CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + CARLA_HOST = os.environ.get("CARLA_HOST", "paf-carla-simulator-1") + CARLA_PORT = int(os.environ.get("CARLA_PORT", "2000")) client = carla.Client(CARLA_HOST, CARLA_PORT) @@ -165,13 +169,14 @@ def spawn_car(distance): # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) bp = blueprint_library.filter("model3")[0] for actor in world.get_actors(): - if actor.attributes.get('role_name') == "hero": + if actor.attributes.get("role_name") == "hero": ego_vehicle = actor break - spawnPoint = carla.Transform(ego_vehicle.get_location() + - carla.Location(y=distance.data), - ego_vehicle.get_transform().rotation) + spawnPoint = carla.Transform( + ego_vehicle.get_location() + carla.Location(y=distance.data), + ego_vehicle.get_transform().rotation, + ) vehicle = world.spawn_actor(bp, spawnPoint) vehicle.set_autopilot(False) @@ -204,7 +209,7 @@ def filter_vision_objects(float_array, oncoming): # Reshape array to 3 columns and n rows (one row per object) float_array = np.asarray(float_array) - float_array = np.reshape(float_array, (float_array.size//3, 3)) + float_array = np.reshape(float_array, (float_array.size // 3, 3)) # Filter all rows that contain np.inf float_array = float_array[~np.any(np.isinf(float_array), axis=1), :] if float_array.size == 0: @@ -217,8 +222,7 @@ def filter_vision_objects(float_array, oncoming): # Get cars that are on our lane if oncoming: - cars_in_front = \ - all_cars[np.where(all_cars[:, 2] > 0.3)] + cars_in_front = all_cars[np.where(all_cars[:, 2] > 0.3)] if cars_in_front.size != 0: cars_in_front = cars_in_front[np.where(cars_in_front[:, 2] < 1.3)] else: diff --git a/code/test-route/src/test_route.py b/code/test-route/src/test_route.py index 8d0e59a2..3e699a35 100755 --- a/code/test-route/src/test_route.py +++ b/code/test-route/src/test_route.py @@ -4,6 +4,7 @@ import ros_compatibility as roscomp from ros_compatibility.node import CompatibleNode import carla + # from carla import command import rospy import random @@ -11,17 +12,18 @@ class TestRoute(CompatibleNode): def __init__(self): - super(TestRoute, self).__init__('testRoute') + super(TestRoute, self).__init__("testRoute") - self.control_loop_rate = self.get_param('control_loop_rate', 0.025) - self.role_name = self.get_param('role_name', 'ego_vehicle') - self.follow_hero = self.get_param('follow_hero', True) - self.vehicle_number = self.get_param('vehicle_number', 50) - self.only_cars = self.get_param('only_cars', False) - self.disable_vehicle_lane_change = \ - self.get_param('disable_vehicle_lane_change', False) + self.control_loop_rate = self.get_param("control_loop_rate", 0.025) + self.role_name = self.get_param("role_name", "ego_vehicle") + self.follow_hero = self.get_param("follow_hero", True) + self.vehicle_number = self.get_param("vehicle_number", 50) + self.only_cars = self.get_param("only_cars", False) + self.disable_vehicle_lane_change = self.get_param( + "disable_vehicle_lane_change", False + ) - host = os.environ.get('CARLA_SIM_HOST', 'localhost') + host = os.environ.get("CARLA_SIM_HOST", "localhost") self.client = carla.Client(host, 2000) self.client.set_timeout(60.0) @@ -49,16 +51,17 @@ def __init__(self): self.spectator = self.world.get_spectator() def spawn_traffic(self): - self.loginfo('Spawning traffic') + self.loginfo("Spawning traffic") spawn_points = self.world.get_map().get_spawn_points() hero_location = self.hero.get_location() spawn_points.sort(key=lambda x: x.location.distance(hero_location)) - blueprints = self.world.get_blueprint_library().filter('vehicle.*') + blueprints = self.world.get_blueprint_library().filter("vehicle.*") if self.only_cars: - blueprints = [b for b in blueprints - if int(b.get_attribute('number_of_wheels')) == 4] + blueprints = [ + b for b in blueprints if int(b.get_attribute("number_of_wheels")) == 4 + ] vehicles = [] max_vehicles = min([self.vehicle_number, len(spawn_points)]) @@ -70,17 +73,19 @@ def spawn_traffic(self): for _, transform in enumerate(spawn_points[:max_vehicles]): blueprint = random.choice(blueprints) - if blueprint.has_attribute('driver_id'): - driver_id = random.choice(blueprint.get_attribute('driver_id') - .recommended_values) - blueprint.set_attribute('driver_id', driver_id) + if blueprint.has_attribute("driver_id"): + driver_id = random.choice( + blueprint.get_attribute("driver_id").recommended_values + ) + blueprint.set_attribute("driver_id", driver_id) - if blueprint.has_attribute('color'): - color = random.choice(blueprint.get_attribute('color') - .recommended_values) - blueprint.set_attribute('color', color) + if blueprint.has_attribute("color"): + color = random.choice( + blueprint.get_attribute("color").recommended_values + ) + blueprint.set_attribute("color", color) - blueprint.set_attribute('role_name', 'autopilot') + blueprint.set_attribute("role_name", "autopilot") vehicle = self.world.try_spawn_actor(blueprint, transform) @@ -100,7 +105,7 @@ def spawn_traffic(self): # else: # vehicles.append(response.actor_id) - self.loginfo('Spawned {} vehicles'.format(len(vehicles))) + self.loginfo("Spawned {} vehicles".format(len(vehicles))) def wait_for_hero(self): while not rospy.is_shutdown(): @@ -108,9 +113,12 @@ def wait_for_hero(self): if not actors: continue - self.hero = [a for a in actors - if 'role_name' in a.attributes and - a.attributes.get('role_name') == self.role_name] + self.hero = [ + a + for a in actors + if "role_name" in a.attributes + and a.attributes.get("role_name") == self.role_name + ] if self.hero: self.hero = self.hero[0] break @@ -118,35 +126,35 @@ def wait_for_hero(self): def set_spectator(self, set_rotation=False): transform = self.hero.get_transform() location = carla.Location( - x=transform.location.x, - y=transform.location.y, - z=transform.location.z + 2) + x=transform.location.x, y=transform.location.y, z=transform.location.z + 2 + ) if set_rotation: self.spectator.set_transform( carla.Transform( - location, carla.Rotation( + location, + carla.Rotation( pitch=transform.rotation.pitch - 15, yaw=transform.rotation.yaw, - roll=transform.rotation.roll - ) + roll=transform.rotation.roll, + ), ) ) else: self.spectator.set_location(location) def run(self): - self.loginfo('Test-Route node running') + self.loginfo("Test-Route node running") self.set_spectator(set_rotation=True) def loop(timer_event=None): self.set_spectator() if self.follow_hero: - self.loginfo('Following hero') + self.loginfo("Following hero") self.new_timer(self.control_loop_rate, loop) else: - self.loginfo('Not following hero, setting spectator only once') + self.loginfo("Not following hero, setting spectator only once") self.set_spectator() sleep(5) @@ -156,7 +164,7 @@ def loop(timer_event=None): def main(args=None): - roscomp.init('testRoute', args=args) + roscomp.init("testRoute", args=args) try: node = TestRoute() @@ -167,5 +175,5 @@ def main(args=None): roscomp.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/doc/development/templates/template_class.py b/doc/development/templates/template_class.py index 164c3585..5267d75c 100644 --- a/doc/development/templates/template_class.py +++ b/doc/development/templates/template_class.py @@ -28,6 +28,7 @@ # two blank lines between top level functions and class definition + ############################# # 3. Class-Defintion # ############################# diff --git a/doc/development/templates/template_class_no_comments.py b/doc/development/templates/template_class_no_comments.py index 13640a5b..365fdcfd 100644 --- a/doc/development/templates/template_class_no_comments.py +++ b/doc/development/templates/template_class_no_comments.py @@ -20,12 +20,12 @@ def test_function1(self, param1): def test_function2(cls): """ - + :return: """ pass - def test_function3(self): # inline comment + def test_function3(self): # inline comment # This is a block comment # It goes over multiple lines # All comments start with a blank space @@ -51,10 +51,10 @@ def test_function5(self, param1, param2): return param1 def main(self): - """_summary_ - """ + """_summary_""" print("Hello World") + if __name__ == "__main__": runner = TestClass() runner.main() diff --git a/doc/perception/experiments/object-detection-model_evaluation/globals.py b/doc/perception/experiments/object-detection-model_evaluation/globals.py index 5cb86c0a..325107dc 100644 --- a/doc/perception/experiments/object-detection-model_evaluation/globals.py +++ b/doc/perception/experiments/object-detection-model_evaluation/globals.py @@ -1,12 +1,12 @@ -IMAGE_BASE_FOLDER = '/home/maxi/paf/code/output/12-dev/rgb/center' +IMAGE_BASE_FOLDER = "/home/maxi/paf/code/output/12-dev/rgb/center" IMAGES_FOR_TEST = { - 'start': '1600.png', - 'intersection': '1619.png', - 'traffic_light': '1626.png', - 'traffic': '1660.png', - 'bicycle_far': '1663.png', - 'bicycle_close': '1668.png', - 'construction_sign_far': '2658.png', - 'construction_sign_close': '2769.png' + "start": "1600.png", + "intersection": "1619.png", + "traffic_light": "1626.png", + "traffic": "1660.png", + "bicycle_far": "1663.png", + "bicycle_close": "1668.png", + "construction_sign_far": "2658.png", + "construction_sign_close": "2769.png", } diff --git a/doc/perception/experiments/object-detection-model_evaluation/pt.py b/doc/perception/experiments/object-detection-model_evaluation/pt.py index 145fbcfe..29ec47d3 100644 --- a/doc/perception/experiments/object-detection-model_evaluation/pt.py +++ b/doc/perception/experiments/object-detection-model_evaluation/pt.py @@ -1,16 +1,16 @@ -''' +""" Docs: https://pytorch.org/vision/stable/models.html#object-detection -''' +""" import os from time import perf_counter import torch import torchvision -from torchvision.models.detection.faster_rcnn import \ - FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, \ - FasterRCNN_ResNet50_FPN_V2_Weights -from torchvision.models.detection.retinanet import \ - RetinaNet_ResNet50_FPN_V2_Weights +from torchvision.models.detection.faster_rcnn import ( + FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, + FasterRCNN_ResNet50_FPN_V2_Weights, +) +from torchvision.models.detection.retinanet import RetinaNet_ResNet50_FPN_V2_Weights from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST from torchvision.utils import draw_bounding_boxes from pathlib import Path @@ -20,28 +20,27 @@ from torchvision.transforms.functional import to_pil_image ALL_MODELS = { - 'fasterrcnn_mobilenet_v3_large_320_fpn': - FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, - 'fasterrcnn_resnet50_fpn_v2': FasterRCNN_ResNet50_FPN_V2_Weights, - 'retinanet_resnet50_fpn_v2': RetinaNet_ResNet50_FPN_V2_Weights, + "fasterrcnn_mobilenet_v3_large_320_fpn": FasterRCNN_MobileNet_V3_Large_320_FPN_Weights, + "fasterrcnn_resnet50_fpn_v2": FasterRCNN_ResNet50_FPN_V2_Weights, + "retinanet_resnet50_fpn_v2": RetinaNet_ResNet50_FPN_V2_Weights, } def load_model(model_name): - print('Selected model: ' + model_name) - print('Loading model...', end='') + print("Selected model: " + model_name) + print("Loading model...", end="") device = torch.device("cuda" if torch.cuda.is_available() else "cpu") weights = ALL_MODELS[model_name].DEFAULT - model = torchvision.models.detection.__dict__[model_name]( - weights=weights - ).to(device) + model = torchvision.models.detection.__dict__[model_name](weights=weights).to( + device + ) model.eval() return model, weights, device def load_image(image_path, model_weights, device): img = Image.open(image_path) - img = img.convert('RGB') + img = img.convert("RGB") img = transforms.Compose([transforms.PILToTensor()])(img) img = model_weights.transforms()(img) img = img.unsqueeze_(0) @@ -60,11 +59,11 @@ def load_image(image_path, model_weights, device): image_np = load_image(image_path, weights, device) if first_gen: - print('Running warmup inference...') + print("Running warmup inference...") model(image_np) first_gen = False - print(f'Running inference for {p}... ') + print(f"Running inference for {p}... ") start_time = perf_counter() @@ -79,21 +78,20 @@ def load_image(image_path, model_weights, device): label_id_offset = -1 - image_np_with_detections = torch.tensor(image_np * 255, - dtype=torch.uint8) - boxes = result['boxes'] - scores = result['scores'] - labels = [weights.meta["categories"][i] for i in result['labels']] + image_np_with_detections = torch.tensor(image_np * 255, dtype=torch.uint8) + boxes = result["boxes"] + scores = result["scores"] + labels = [weights.meta["categories"][i] for i in result["labels"]] - box = draw_bounding_boxes(image_np_with_detections[0], boxes, labels, - colors='red', width=2) + box = draw_bounding_boxes( + image_np_with_detections[0], boxes, labels, colors="red", width=2 + ) box_img = to_pil_image(box) file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms', - fontsize=30) + plt.title(f"PyTorch - {m} - {p} - {elapsed_time*1000:.0f}ms", fontsize=30) plt.imshow(box_img) - plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_PT_{m}.jpg') + plt.savefig(f"{IMAGE_BASE_FOLDER}/result/{file_name}_PT_{m}.jpg") plt.close() diff --git a/doc/perception/experiments/object-detection-model_evaluation/pylot.py b/doc/perception/experiments/object-detection-model_evaluation/pylot.py index d59e5e75..19e2a3b1 100644 --- a/doc/perception/experiments/object-detection-model_evaluation/pylot.py +++ b/doc/perception/experiments/object-detection-model_evaluation/pylot.py @@ -1,7 +1,7 @@ -''' +""" Docs: https://www.tensorflow.org/hub/tutorials/tf2_object_detection, https://pylot.readthedocs.io/en/latest/perception.detection.html -''' +""" from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST @@ -20,55 +20,57 @@ from object_detection.utils import visualization_utils as viz_utils -matplotlib.use('TkAgg') +matplotlib.use("TkAgg") -tf.get_logger().setLevel('ERROR') +tf.get_logger().setLevel("ERROR") ALL_MODELS = [ - 'faster-rcnn', - 'ssdlite-mobilenet-v2', - 'ssd-mobilenet-fpn-640', - 'ssd-mobilenet-v1', - 'ssd-mobilenet-v1-fpn' + "faster-rcnn", + "ssdlite-mobilenet-v2", + "ssd-mobilenet-fpn-640", + "ssd-mobilenet-v1", + "ssd-mobilenet-v1-fpn", ] -MODEL_BASE_FOLDER = '/home/maxi/Downloads/models/obstacle_detection' +MODEL_BASE_FOLDER = "/home/maxi/Downloads/models/obstacle_detection" -LABEL_FILE = '/home/maxi/Downloads/pylot.names' +LABEL_FILE = "/home/maxi/Downloads/pylot.names" def load_image_into_numpy_array(path): - image_data = tf.io.gfile.GFile(path, 'rb').read() + image_data = tf.io.gfile.GFile(path, "rb").read() image = Image.open(BytesIO(image_data)) (im_width, im_height) = image.size - return np.array(image.convert('RGB').getdata()).reshape( - (1, im_height, im_width, 3)).astype(np.uint8) + return ( + np.array(image.convert("RGB").getdata()) + .reshape((1, im_height, im_width, 3)) + .astype(np.uint8) + ) def load_model(model_name): model_handle = os.path.join(MODEL_BASE_FOLDER, model_name) - print('Selected model: ' + model_name) + print("Selected model: " + model_name) - print('Loading model...', end='') + print("Loading model...", end="") hub_model = hub.load(model_handle) - print(' done!') + print(" done!") return hub_model def get_category_index(label_file): - with open(label_file, 'r') as f: + with open(label_file, "r") as f: labels = f.readlines() labels = [label.strip() for label in labels] - category_index = \ - {i: {'id': i, 'name': name} for i, name in enumerate(labels)} + category_index = {i: {"id": i, "name": name} for i, name in enumerate(labels)} return category_index -if not os.path.exists(f'{IMAGE_BASE_FOLDER}/result'): - os.makedirs(f'{IMAGE_BASE_FOLDER}/result') +if not os.path.exists(f"{IMAGE_BASE_FOLDER}/result"): + os.makedirs(f"{IMAGE_BASE_FOLDER}/result") category_index = get_category_index(LABEL_FILE) @@ -82,16 +84,16 @@ def get_category_index(label_file): image_tensor = tf.convert_to_tensor(image_np) if first_gen: - print('Running warmup inference...') - model.signatures['serving_default'](image_tensor) + print("Running warmup inference...") + model.signatures["serving_default"](image_tensor) first_gen = False - print(f'Running inference for {p}... ') + print(f"Running inference for {p}... ") start_time = perf_counter() # running inference - results = model.signatures['serving_default'](image_tensor) + results = model.signatures["serving_default"](image_tensor) elapsed_time = perf_counter() - start_time @@ -104,20 +106,20 @@ def get_category_index(label_file): viz_utils.visualize_boxes_and_labels_on_image_array( image_np_with_detections[0], - result['boxes'][0], - (result['classes'][0] + label_id_offset).astype(int), - result['scores'][0], + result["boxes"][0], + (result["classes"][0] + label_id_offset).astype(int), + result["scores"][0], category_index, use_normalized_coordinates=True, max_boxes_to_draw=200, - min_score_thresh=.10, - agnostic_mode=False) + min_score_thresh=0.10, + agnostic_mode=False, + ) file_name = Path(image_path).stem plt.figure(figsize=(32, 18)) - plt.title(f'Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms', - fontsize=30) + plt.title(f"Pylot (TF) - {m} - {p} - {elapsed_time*1000:.0f}ms", fontsize=30) plt.imshow(image_np_with_detections[0]) - plt.savefig(f'{IMAGE_BASE_FOLDER}/result/{file_name}_TF_{m}.jpg') + plt.savefig(f"{IMAGE_BASE_FOLDER}/result/{file_name}_TF_{m}.jpg") plt.close() diff --git a/doc/perception/experiments/object-detection-model_evaluation/yolo.py b/doc/perception/experiments/object-detection-model_evaluation/yolo.py index 39d727b7..b4949622 100644 --- a/doc/perception/experiments/object-detection-model_evaluation/yolo.py +++ b/doc/perception/experiments/object-detection-model_evaluation/yolo.py @@ -1,9 +1,9 @@ -''' +""" Docs: https://docs.ultralytics.com/modes/predict/ https://docs.ultralytics.com/tasks/detect/#models https://docs.ultralytics.com/models/yolo-nas -''' +""" import os from globals import IMAGE_BASE_FOLDER, IMAGES_FOR_TEST @@ -12,33 +12,34 @@ import torch ALL_MODELS = { - 'yolov8n': YOLO, - 'yolov8s': YOLO, - 'yolov8m': YOLO, - 'yolov8l': YOLO, - 'yolov8x': YOLO, - 'yolo_nas_l': NAS, - 'yolo_nas_m': NAS, - 'yolo_nas_s': NAS, - 'rtdetr-l': RTDETR, - 'rtdetr-x': RTDETR, - 'yolov8x-seg': YOLO, - 'sam-l': SAM, - 'FastSAM-x': FastSAM, + "yolov8n": YOLO, + "yolov8s": YOLO, + "yolov8m": YOLO, + "yolov8l": YOLO, + "yolov8x": YOLO, + "yolo_nas_l": NAS, + "yolo_nas_m": NAS, + "yolo_nas_s": NAS, + "rtdetr-l": RTDETR, + "rtdetr-x": RTDETR, + "yolov8x-seg": YOLO, + "sam-l": SAM, + "FastSAM-x": FastSAM, } with torch.inference_mode(): for m, wrapper in ALL_MODELS.items(): - print('Selected model: ' + m) - model_path = os.path.join('yolo', m + '.pt') + print("Selected model: " + m) + model_path = os.path.join("yolo", m + ".pt") model = wrapper(model_path) for p in IMAGES_FOR_TEST: image_path = os.path.join(IMAGE_BASE_FOLDER, IMAGES_FOR_TEST[p]) img = Image.open(image_path) - _ = model.predict(source=img, save=True, save_conf=True, - line_width=1, half=True) + _ = model.predict( + source=img, save=True, save_conf=True, line_width=1, half=True + ) del model diff --git a/doc/research/paf23/planning/test_traj.py b/doc/research/paf23/planning/test_traj.py index 97283e6c..1e9edc71 100644 --- a/doc/research/paf23/planning/test_traj.py +++ b/doc/research/paf23/planning/test_traj.py @@ -1,18 +1,22 @@ -from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ - import run_fot +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper import ( + run_fot, +) import numpy as np import matplotlib.pyplot as plt -wp = wp = np.r_[[np.full((50), 983.5889666959667)], - [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T +wp = wp = np.r_[ + [np.full((50), 983.5889666959667)], + [np.linspace(5370.016106881272, 5399.016106881272, 50)], +].T initial_conditions = { - 'ps': 0, - 'target_speed': 6, - 'pos': np.array([983.5807552562393, 5370.014637890163]), - 'vel': np.array([5, 1]), - 'wp': wp, - 'obs': np.array([[983.568124548765, 5386.0219828457075, - 983.628124548765, 5386.0219828457075]]) + "ps": 0, + "target_speed": 6, + "pos": np.array([983.5807552562393, 5370.014637890163]), + "vel": np.array([5, 1]), + "wp": wp, + "obs": np.array( + [[983.568124548765, 5386.0219828457075, 983.628124548765, 5386.0219828457075]] + ), } hyperparameters = { @@ -39,9 +43,21 @@ "num_threads": 0, # set 0 to avoid using threaded algorithm } -result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) +( + result_x, + result_y, + speeds, + ix, + iy, + iyaw, + d, + s, + speeds_x, + speeds_y, + misc, + costs, + success, +) = run_fot(initial_conditions, hyperparameters) if success: print("Success!") @@ -50,12 +66,18 @@ fig, ax = plt.subplots(1, 2) ax[0].scatter(wp[:, 0], wp[:, 1], label="original") - ax[0].scatter([983.568124548765, 983.628124548765], - [5386.0219828457075, 5386.0219828457075], label="object") + ax[0].scatter( + [983.568124548765, 983.628124548765], + [5386.0219828457075, 5386.0219828457075], + label="object", + ) ax[0].set_xticks([983.518124548765, 983.598124548765]) ax[1].scatter(result_x, result_y, label="frenet") - ax[1].scatter([983.568124548765, 983.628124548765], - [5386.0219828457075, 5386.0219828457075], label="object") + ax[1].scatter( + [983.568124548765, 983.628124548765], + [5386.0219828457075, 5386.0219828457075], + label="object", + ) ax[1].set_xticks([983.518124548765, 983.598124548765]) plt.legend() plt.show() From daf2255f217aeec3654f2747c56dba298ec23c93 Mon Sep 17 00:00:00 2001 From: JulianTrommer Date: Tue, 15 Oct 2024 15:56:49 +0200 Subject: [PATCH 5/5] Refactored repo with linters --- .vscode/settings.json | 1 - build/agent_service.yaml | 4 ++ build/docker-compose.dev.yaml | 4 +- build/docker/agent-dev/dev_entrypoint.sh | 16 +---- build/docker/agent/Dockerfile | 3 +- build/docker/agent/Dockerfile_Submission | 35 ++++----- build/docker/agent/entrypoint.sh | 4 +- code/__init__.py | 0 code/perception/launch/perception.launch | 4 +- .../traffic_light_training.py | 8 +-- code/perception/src/traffic_light_node.py | 4 +- code/perception/src/vision_node.py | 4 +- code/planning/__init__.py | 1 - code/planning/src/behavior_agent/__init__.py | 0 .../src/behavior_agent/behavior_tree.py | 72 ++++++++----------- .../src/behavior_agent/behaviours/__init__.py | 3 - .../behavior_agent/behaviours/intersection.py | 12 ++-- .../behavior_agent/behaviours/lane_change.py | 2 +- .../behavior_agent/behaviours/maneuvers.py | 2 +- .../src/behavior_agent/behaviours/overtake.py | 3 +- .../src/local_planner/motion_planning.py | 7 +- doc/development/templates/template_class.py | 17 +++-- .../templates/template_class_no_comments.py | 8 +-- .../object-detection-model_evaluation/pt.py | 4 +- 24 files changed, 101 insertions(+), 117 deletions(-) delete mode 100644 code/__init__.py delete mode 100755 code/planning/__init__.py delete mode 100755 code/planning/src/behavior_agent/__init__.py mode change 100755 => 100644 code/planning/src/behavior_agent/behaviours/__init__.py diff --git a/.vscode/settings.json b/.vscode/settings.json index 817fbe31..260ba70c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,7 +1,6 @@ { "githubIssues.issueBranchTitle": "${issueNumber}-${sanitizedIssueTitle}", "githubIssues.queries": [ - { "label": "My Issues", "query": "default" diff --git a/build/agent_service.yaml b/build/agent_service.yaml index 8266fe9d..13d8bab1 100644 --- a/build/agent_service.yaml +++ b/build/agent_service.yaml @@ -3,6 +3,10 @@ services: build: dockerfile: build/docker/agent/Dockerfile context: ../ + args: + USERNAME: ${USERNAME} + USER_UID: ${USER_UID} + USER_GID: ${USER_GID} init: true tty: true shm_size: 2gb diff --git a/build/docker-compose.dev.yaml b/build/docker-compose.dev.yaml index 95d06f11..ab21bf03 100644 --- a/build/docker-compose.dev.yaml +++ b/build/docker-compose.dev.yaml @@ -1,5 +1,8 @@ # compose file for the development without a driving vehicle # "interactive" development without a car +include: + - roscore_service.yaml + services: agent-dev: build: @@ -24,5 +27,4 @@ services: - DISPLAY=${DISPLAY} network_mode: host privileged: true - entrypoint: ["/dev_entrypoint.sh"] command: bash -c "sudo chown -R ${USER_UID}:${USER_GID} ../ && sudo chmod -R a+w ../ && bash" diff --git a/build/docker/agent-dev/dev_entrypoint.sh b/build/docker/agent-dev/dev_entrypoint.sh index 14f912e3..2626fcb9 100755 --- a/build/docker/agent-dev/dev_entrypoint.sh +++ b/build/docker/agent-dev/dev_entrypoint.sh @@ -1,19 +1,7 @@ #!/bin/bash +set -e -# Source ROS setup source /opt/ros/noetic/setup.bash - -# Source the catkin workspace setup source /catkin_ws/devel/setup.bash -# Set up any additional environment variables if needed -export CARLA_ROOT=/opt/carla -export SCENARIO_RUNNER_ROOT=/opt/scenario_runner -export LEADERBOARD_ROOT=/opt/leaderboard - -# Execute the command passed to the script, or start a bash session if no command was given -if [ $# -eq 0 ]; then - exec bash -else - exec "$@" -fi \ No newline at end of file +exec "$@" diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index c8c05ceb..dff54814 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -170,12 +170,11 @@ RUN source /opt/ros/noetic/setup.bash && catkin_make ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh - - # set the default working directory to the code WORKDIR /workspace/code RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc +RUN echo "source /catkin_ws/devel/setup.bash" >> ~/.bashrc ENTRYPOINT ["/entrypoint.sh"] CMD ["bash", "-c", "sleep 10 && \ diff --git a/build/docker/agent/Dockerfile_Submission b/build/docker/agent/Dockerfile_Submission index 128a8bd8..8128266e 100644 --- a/build/docker/agent/Dockerfile_Submission +++ b/build/docker/agent/Dockerfile_Submission @@ -19,7 +19,7 @@ ARG DEBIAN_FRONTEND=noninteractive # install rendering dependencies for rviz / rqt RUN apt-get update \ - && apt-get install -y -qq --no-install-recommends \ + && apt-get install -y -qq --no-install-recommends \ libxext6 libx11-6 libglvnd0 libgl1 \ libglx0 libegl1 freeglut3-dev @@ -27,10 +27,10 @@ RUN apt-get update \ RUN apt-get install wget unzip RUN wget https://github.com/una-auxme/paf/releases/download/v0.0.1/PythonAPI_Leaderboard-2.0.zip -O PythonAPI.zip \ - && unzip PythonAPI.zip \ - && rm PythonAPI.zip \ - && mkdir -p /opt/carla \ - && mv PythonAPI /opt/carla/PythonAPI + && unzip PythonAPI.zip \ + && rm PythonAPI.zip \ + && mkdir -p /opt/carla \ + && mv PythonAPI /opt/carla/PythonAPI # build libgit2 RUN wget https://github.com/libgit2/libgit2/archive/refs/tags/v1.5.0.tar.gz -O libgit2-1.5.0.tar.gz \ @@ -67,12 +67,12 @@ ENV PYTHONPATH=$PYTHONPATH:/opt/carla/PythonAPI/carla/dist/carla-0.9.14-py3.7-li # install mlocate, pip, wget, git and some ROS dependencies for building the CARLA ROS bridge RUN apt-get update && apt-get install -y \ - mlocate python3-pip wget git python-is-python3 \ - ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ - ros-noetic-carla-msgs ros-noetic-pcl-conversions \ - ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ - ros-noetic-robot-pose-ekf ros-noetic-ros-numpy \ - ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure + mlocate python3-pip wget git python-is-python3 \ + ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ + ros-noetic-carla-msgs ros-noetic-pcl-conversions \ + ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ + ros-noetic-robot-pose-ekf ros-noetic-ros-numpy \ + ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure SHELL ["/bin/bash", "-c"] @@ -178,12 +178,13 @@ ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh WORKDIR /workspace/code RUN echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc +RUN echo "source /catkin_ws/devel/setup.bash" >> ~/.bashrc ENTRYPOINT ["/entrypoint.sh"] CMD ["bash", "-c", "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=${DEBUG_CHALLENGE} \ ---repetitions=${REPETITIONS} \ ---checkpoint=${CHECKPOINT_ENDPOINT} \ ---track=${CHALLENGE_TRACK} \ ---agent=${TEAM_AGENT} \ ---routes=${ROUTES} \ ---host=${CARLA_SIM_HOST}"] + --repetitions=${REPETITIONS} \ + --checkpoint=${CHECKPOINT_ENDPOINT} \ + --track=${CHALLENGE_TRACK} \ + --agent=${TEAM_AGENT} \ + --routes=${ROUTES} \ + --host=${CARLA_SIM_HOST}"] diff --git a/build/docker/agent/entrypoint.sh b/build/docker/agent/entrypoint.sh index 61e51dc4..2626fcb9 100755 --- a/build/docker/agent/entrypoint.sh +++ b/build/docker/agent/entrypoint.sh @@ -1,7 +1,7 @@ #!/bin/bash set -e -source "/opt/ros/noetic/setup.bash" -source "/catkin_ws/devel/setup.bash" +source /opt/ros/noetic/setup.bash +source /catkin_ws/devel/setup.bash exec "$@" diff --git a/code/__init__.py b/code/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 45d970be..8d6072e7 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -41,8 +41,8 @@