diff --git a/.github/workflows/BuildAndRun.yaml b/.github/workflows/BuildAndRun.yaml index c8f9678e5c4..fdf30f65e2a 100644 --- a/.github/workflows/BuildAndRun.yaml +++ b/.github/workflows/BuildAndRun.yaml @@ -61,25 +61,28 @@ jobs: run: | vcs import src < src/scenario_simulator_v2/dependency_${{ matrix.rosdistro }}.repos - - name: Resolve rosdep + - name: Resolve rosdep and install colcon mixin run: | apt-get update - apt-get install -y python3-pip + apt-get install -y python3-pip python3-colcon-lcov-result lcov rosdep update --include-eol-distros rosdep install -iy --from-paths src --rosdistro ${{ matrix.rosdistro }} + colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml + colcon mixin update default shell: bash - name: Build packages run: | source /opt/ros/${{ matrix.rosdistro }}/setup.bash - colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_CPP_MOCK_SCENARIOS=ON --packages-up-to ${{ steps.list_packages.outputs.package_list }} + colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_CPP_MOCK_SCENARIOS=ON -DBUILD_TESTING=true -DCMAKE_CXX_FLAGS='-fprofile-arcs -ftest-coverage' -DCMAKE_C_FLAGS='-fprofile-arcs -ftest-coverage' --packages-up-to ${{ steps.list_packages.outputs.package_list }} shell: bash - name: Colcon test run: | source /opt/ros/${{ matrix.rosdistro }}/setup.bash source install/local_setup.bash - colcon test --packages-select ${{ steps.list_packages.outputs.package_list }} + colcon test --event-handlers console_cohesion+ --packages-select ${{ steps.list_packages.outputs.package_list }} + colcon lcov-result --packages-select ${{ steps.list_packages.outputs.package_list }} shell: bash - name: Show test result @@ -97,6 +100,13 @@ jobs: ./src/scenario_simulator_v2/.github/workflows/workflow.sh ./src/scenario_simulator_v2/test_runner/scenario_test_runner/config/workflow.txt global_frame_rate:=20 shell: bash + - name: Upload Lcov result + uses: actions/upload-artifact@v4 + with: + name: lcov + path: lcov + retention-days: 1 + # - name: Basic test # run: | # source install/setup.bash diff --git a/.github/workflows/Docker.yaml b/.github/workflows/Docker.yaml new file mode 100644 index 00000000000..4c2cad55c6e --- /dev/null +++ b/.github/workflows/Docker.yaml @@ -0,0 +1,81 @@ +name: Docker +on: + pull_request: + paths: + - "**" + - "!docs/**" + - "!README.md" + - "!CONTRIBUTING.md" + - "!.github/**" + - ".github/workflows/Docker.yaml" + - "!mkdocs.yml" + - "!pyproject.toml" + - "!poetry.lock" + workflow_dispatch: + inputs: + version: + description: version of the scenario_simulator_v2 + required: true + +jobs: + push_docker: + name: Push Docker Image + runs-on: ubuntu-22.04 + timeout-minutes: 720 + strategy: + matrix: + rosdistro: [humble] + arch: [amd64] + # Build test for arm64 CPU is broken. + # This is a temporary solution and will be repaired in the future. + # See also https://github.com/tier4/scenario_simulator_v2/pull/1295 + # arch: [amd64, arm64] + steps: + - name: Free Disk Space (Ubuntu) + uses: jlumbroso/free-disk-space@main + with: + tool-cache: false + + - name: Install docker for ubuntu runner + uses: docker/setup-buildx-action@v3 + + - name: Install QEMU + uses: docker/setup-qemu-action@v3 + + - uses: actions/checkout@v4 + + - name: Login to GitHub Container Registry + uses: docker/login-action@v2 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Build (${{ matrix.arch }}) + if: github.event_name == 'pull_request' + uses: docker/bake-action@v3 + with: + files: | + ./docker-bake.hcl + workdir: . + set: | + *.cache-to=type=gha,mode=max + *.cache-from=type=gha + push: false + targets: | + ${{ matrix.rosdistro }}_base_${{ matrix.arch }} + + - name: Build and push (${{ matrix.arch }}) + if: github.event_name == 'workflow_dispatch' + uses: docker/bake-action@v3 + with: + files: | + ./docker-bake.hcl + workdir: . + set: | + *.cache-to=type=gha,mode=max + *.cache-from=type=gha + *.tags=ghcr.io/tier4/scenario_simulator_v2:humble-${{ github.event.inputs.version }} + push: true + targets: | + ${{ matrix.rosdistro }}_base_${{ matrix.arch }} diff --git a/.github/workflows/Release.yaml b/.github/workflows/Release.yaml index fbbc78ff620..7fd8180a2b9 100644 --- a/.github/workflows/Release.yaml +++ b/.github/workflows/Release.yaml @@ -36,7 +36,7 @@ jobs: uses: levonet/action-restore-branch@master - name: Install bloom - run: apt update && apt install -y python3-bloom git + run: apt update && apt install -y python3-bloom git curl - name: Checkout uses: actions/checkout@v4 @@ -142,50 +142,14 @@ jobs: repo: context.repo.repo, ref: `heads/${context.payload.pull_request.head.ref}`, }) - push_docker: - needs: release - name: Push Docker Image - runs-on: ubuntu-22.04 - timeout-minutes: 720 - strategy: - matrix: - rosdistro: [humble] - arch: [amd64] - # Build test for arm64 CPU is broken. - # This is a temporary solution and will be repaired in the future. - # See also https://github.com/tier4/scenario_simulator_v2/pull/1295 - # arch: [amd64, arm64] - steps: - - name: Free Disk Space (Ubuntu) - uses: jlumbroso/free-disk-space@main - with: - tool-cache: false - - - name: Install docker for ubuntu runner - uses: docker/setup-buildx-action@v3 - - - name: Install QEMU - uses: docker/setup-qemu-action@v3 - - - uses: actions/checkout@v3 - - name: Login to GitHub Container Registry - uses: docker/login-action@v2 - with: - registry: ghcr.io - username: ${{ github.actor }} - password: ${{ secrets.GITHUB_TOKEN }} - - - name: Build and push (${{ matrix.arch }}) - uses: docker/bake-action@v3 - with: - files: | - ./docker-bake.hcl - workdir: . - set: | - *.cache-to=type=gha,mode=max - *.cache-from=type=gha - *.tags=ghcr.io/tier4/scenario_simulator_v2:humble-${{ needs.release.outputs.new_version }} - push: ${{ github.event.pull_request.merged == true }} - targets: | - ${{ matrix.rosdistro }}_base_${{ matrix.arch }} + - name: Kick docker build action + if: github.event.pull_request.merged == true + run: | + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: token ${{ secrets.BLOOM_GITHUB_TOKEN }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/tier4/scenario_simulator_v2/actions/workflows/Docker.yaml/dispatches \ + -d '{"ref":"master","inputs":{"version":"${{ steps.new_version.outputs.new_version }}"}}' diff --git a/.github/workflows/custom_spell.json b/.github/workflows/custom_spell.json index 32db063ea4e..68b3af530c5 100644 --- a/.github/workflows/custom_spell.json +++ b/.github/workflows/custom_spell.json @@ -5,7 +5,9 @@ "AUTORCC", "Canonicalized", "canonicalizing", + "cfamily", "classname", + "Cmyk", "Dawid", "DBOOST", "DBUILD", @@ -14,11 +16,8 @@ "DWITH_INTEGRATION_TEST", "engageable", "euclidian", - "hakuturu", - "Kataoka", "libunwind", "linelint", - "Masaya", "Mersenne", "Monic", "Moszynski", @@ -40,8 +39,8 @@ "TESTRANDOMIZER", "travelling", "Tschirnhaus", + "walltime", "xerces", - "xercesc", - "yamacir-kit" + "xercesc" ] } diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 991cd182db2..a3f880e8430 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 920c1f5df8a..3ffb010161d 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 3.3.0 + 4.3.7 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index b6182992b5d..69ce526f07d 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,326 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge pull request `#1341 `_ from tier4/RJD-1278/geometry-update + Rjd 1278/geometry update +* Merge branch 'master' into RJD-1278/geometry-update +* updates after merge +* update testcase +* remove empty line +* isIntersect2D_collinear +* refactor toPolygon2D tests +* add comments +* bounding_box clean up +* clean up vector3 +* rename tests in HermiteCurve +* rename tests in CatmullRomSpline +* quaternion operators +* tune down numbers +* sort tests, rm old duplicate +* getIntersection2D function +* getIntersection2DSValue and isIntersect2D functions +* getPose, getPoint refactor +* add a proper structure to the test files +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* update testcase +* remove empty line +* isIntersect2D_collinear +* Merge branch 'master' into RJD-1278/geometry-update +* refactor toPolygon2D tests +* add comments +* bounding_box clean up +* clean up vector3 +* rename tests in HermiteCurve +* rename tests in CatmullRomSpline +* quaternion operators +* tune down numbers +* sort tests, rm old duplicate +* getIntersection2D function +* getIntersection2DSValue and isIntersect2D functions +* getPose, getPoint refactor +* add a proper structure to the test files +* Contributors: Masaya Kataoka, Michał Ciasnocha, robomic + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge pull request `#1353 `_ from tier4/RJD-1278/fix-line-segment + Rjd 1278/fix line segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* make const members public +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* remove{} +* expand on the note, add else to if-stmts +* note +* add else to if statements +* rename getSlope, add consts +* remove unnecessary lambda +* simplify denormalize logic +* use has_value +* rename getIntersection2DSValue, minor logical fixes regarding 2D vs 3D +* LineSegment 2D vs 3D indistinction fixes +* return const& and remove implicit conversions +* vector fields for LineSegment class, cleanup +* use isInBounds function +* combine 2 PR, apply review requests +* Merge branch 'RJD-1278/fix-1344-getIntersection2DSValue' of github.com:tier4/scenario_simulator_v2 into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* isIntesect2D initial solution +* 1344 fix initial solution +* Contributors: Masaya Kataoka, Michał Ciasnocha, robomic + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/common/math/geometry/include/geometry/polygon/line_segment.hpp b/common/math/geometry/include/geometry/polygon/line_segment.hpp index ec17419f31d..8ba0f98bc0c 100644 --- a/common/math/geometry/include/geometry/polygon/line_segment.hpp +++ b/common/math/geometry/include/geometry/polygon/line_segment.hpp @@ -29,6 +29,13 @@ namespace geometry class LineSegment { public: + const geometry_msgs::msg::Point start_point; + const geometry_msgs::msg::Point end_point; + const geometry_msgs::msg::Vector3 vector; + const geometry_msgs::msg::Vector3 vector_2d; + const double length; + const double length_2d; + LineSegment( const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Point & end_point); LineSegment( @@ -36,36 +43,34 @@ class LineSegment double length); ~LineSegment(); LineSegment & operator=(const LineSegment &); - const geometry_msgs::msg::Point start_point; - const geometry_msgs::msg::Point end_point; + + auto isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool; + auto isIntersect2D(const LineSegment & line) const -> bool; + auto isInBounds2D(const geometry_msgs::msg::Point & point) const -> bool; + auto getPoint(const double s, const bool denormalize_s = false) const -> geometry_msgs::msg::Point; auto getPose(const double s, const bool denormalize_s = false, const bool fill_pitch = true) const -> geometry_msgs::msg::Pose; - auto isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool; - auto isIntersect2D(const LineSegment & l0) const -> bool; - auto getIntersection2DSValue( + auto get2DIntersectionSValue( const geometry_msgs::msg::Point & point, const bool denormalize_s = false) const -> std::optional; - auto getIntersection2DSValue(const LineSegment & line, const bool denormalize_s = false) const + auto get2DIntersectionSValue(const LineSegment & line, const bool denormalize_s = false) const -> std::optional; auto getIntersection2D(const LineSegment & line) const -> std::optional; auto getSValue( const geometry_msgs::msg::Pose & pose, double threshold_distance, bool denormalize_s) const -> std::optional; - auto getVector() const -> geometry_msgs::msg::Vector3; auto getNormalVector() const -> geometry_msgs::msg::Vector3; - auto get2DVector() const -> geometry_msgs::msg::Vector3; - auto getLength() const -> double; - auto get2DLength() const -> double; - auto getSlope() const -> double; + auto get2DVectorSlope() const -> double; auto getSquaredDistanceIn2D( const geometry_msgs::msg::Point & point, const double s, const bool denormalize_s = false) const -> double; auto getSquaredDistanceVector( const geometry_msgs::msg::Point & point, const double s, const bool denormalize_s = false) const -> geometry_msgs::msg::Vector3; + auto relativePointPosition2D(const geometry_msgs::msg::Point & point) const -> int; private: auto denormalize(const std::optional & s, const bool throw_error_on_out_of_range = true) diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index a6521ca64fa..de97bac5c30 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 3.3.0 + 4.3.7 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/math/geometry/src/intersection/intersection.cpp b/common/math/geometry/src/intersection/intersection.cpp index 07a32df0b9a..85f0b691809 100644 --- a/common/math/geometry/src/intersection/intersection.cpp +++ b/common/math/geometry/src/intersection/intersection.cpp @@ -13,7 +13,9 @@ // limitations under the License. #include +#include #include +#include namespace math { @@ -21,22 +23,23 @@ namespace geometry { bool isIntersect2D(const LineSegment & line0, const LineSegment & line1) { - double s, t; - s = (line0.start_point.x - line0.end_point.x) * (line1.start_point.y - line0.start_point.y) - - (line0.start_point.y - line0.end_point.y) * (line1.start_point.x - line0.start_point.x); - t = (line0.start_point.x - line0.end_point.x) * (line1.end_point.y - line0.start_point.y) - - (line0.start_point.y - line0.end_point.y) * (line1.end_point.x - line0.start_point.x); - if (s * t > 0) { - return false; - } - s = (line1.start_point.x - line1.end_point.x) * (line0.start_point.y - line1.start_point.y) - - (line1.start_point.y - line1.end_point.y) * (line0.start_point.x - line1.start_point.x); - t = (line1.start_point.x - line1.end_point.x) * (line0.end_point.y - line1.start_point.y) - - (line1.start_point.y - line1.end_point.y) * (line0.end_point.x - line1.start_point.x); - if (s * t > 0) { - return false; + const auto &p0 = line0.start_point, &q0 = line0.end_point; + const auto &p1 = line1.start_point, &q1 = line1.end_point; + + const int relative_position_p0 = line1.relativePointPosition2D(p0); + const int relative_position_q0 = line1.relativePointPosition2D(q0); + const int relative_position_p1 = line0.relativePointPosition2D(p1); + const int relative_position_q1 = line0.relativePointPosition2D(q1); + + if ( + relative_position_p1 == 0 && relative_position_q1 == 0 && relative_position_p0 == 0 && + relative_position_q0 == 0) { + return line0.isInBounds2D(p1) || line0.isInBounds2D(q1) || line1.isInBounds2D(p0) || + line1.isInBounds2D(q0); + } else { + return relative_position_p1 != relative_position_q1 && + relative_position_p0 != relative_position_q0; } - return true; } bool isIntersect2D(const std::vector & lines) @@ -54,21 +57,35 @@ bool isIntersect2D(const std::vector & lines) std::optional getIntersection2D( const LineSegment & line0, const LineSegment & line1) { - if (!isIntersect2D(line0, line1)) { + if (not line0.isIntersect2D(line1)) { return std::nullopt; + } else { + // 'line0' represented as a0x + b0y = c0 + const double a0 = line0.vector_2d.y; + const double b0 = -line0.vector_2d.x; + const double c0 = a0 * line0.start_point.x + b0 * line0.start_point.y; + + // 'line1' represented as a1x + b1y = c1 + const double a1 = line1.vector_2d.y; + const double b1 = -line1.vector_2d.x; + const double c1 = a1 * line1.start_point.x + b1 * line1.start_point.y; + + const double determinant = a0 * b1 - a1 * b0; + + if (std::abs(determinant) <= std::numeric_limits::epsilon()) { + // The lines do intersect, but they are collinear and overlap. + THROW_SIMULATION_ERROR( + "Line segments are collinear. So determinant is zero.", + "If this message was displayed, something completely unexpected happens.", + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } else { + return geometry_msgs::build() + .x((b1 * c0 - b0 * c1) / determinant) + .y((a0 * c1 - a1 * c0) / determinant) + .z(0.0); + } } - const auto det = - (line0.start_point.x - line0.end_point.x) * (line1.end_point.y - line1.start_point.y) - - (line1.end_point.x - line1.start_point.x) * (line0.start_point.y - line0.end_point.y); - const auto t = - ((line1.end_point.y - line1.start_point.y) * (line1.end_point.x - line0.end_point.x) + - (line1.start_point.x - line1.end_point.x) * (line1.end_point.y - line0.end_point.y)) / - det; - geometry_msgs::msg::Point point; - point.x = t * line0.start_point.x + (1.0 - t) * line0.end_point.x; - point.y = t * line0.start_point.y + (1.0 - t) * line0.end_point.y; - point.z = t * line0.start_point.z + (1.0 - t) * line0.end_point.z; - return point; } std::vector getIntersection2D(const std::vector & lines) diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp index 5238a24b307..b3b71a13b13 100644 --- a/common/math/geometry/src/polygon/line_segment.cpp +++ b/common/math/geometry/src/polygon/line_segment.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include #include @@ -28,27 +29,37 @@ namespace geometry { LineSegment::LineSegment( const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Point & end_point) -: start_point(start_point), end_point(end_point) +: start_point(start_point), + end_point(end_point), + vector(geometry_msgs::build() + .x(end_point.x - start_point.x) + .y(end_point.y - start_point.y) + .z(end_point.z - start_point.z)), + vector_2d(geometry_msgs::build() + .x(end_point.x - start_point.x) + .y(end_point.y - start_point.y) + .z(0.0)), + length(hypot(end_point, start_point)), + length_2d(std::hypot(end_point.x - start_point.x, end_point.y - start_point.y)) { } LineSegment::LineSegment( const geometry_msgs::msg::Point & start_point, const geometry_msgs::msg::Vector3 & vec, - double length) -: start_point(start_point), end_point([&]() -> geometry_msgs::msg::Point { - geometry_msgs::msg::Point ret; - double vec_size = std::hypot(vec.x, vec.y); - if (vec_size == 0.0) { + const double length) +: LineSegment::LineSegment(start_point, [&]() -> geometry_msgs::msg::Point { + if (const double vec_size = std::hypot(vec.x, vec.y); vec_size == 0.0) { THROW_SIMULATION_ERROR( "Invalid vector is specified, while constructing LineSegment. ", "The vector should have a non zero length to initialize the line segment correctly. ", "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); + } else { + return geometry_msgs::build() + .x(start_point.x + vec.x / vec_size * length) + .y(start_point.y + vec.y / vec_size * length) + .z(start_point.z + vec.z / vec_size * length); } - ret.x = start_point.x + vec.x / vec_size * length; - ret.y = start_point.y + vec.y / vec_size * length; - ret.z = start_point.z + vec.z / vec_size * length; - return ret; }()) { } @@ -64,25 +75,24 @@ LineSegment::~LineSegment() {} auto LineSegment::getPoint(const double s, const bool denormalize_s) const -> geometry_msgs::msg::Point { - const double s_normalized = denormalize_s ? s / getLength() : s; - if (0 <= s_normalized && s_normalized <= 1) { + const double s_normalized = denormalize_s ? s / length : s; + if (0.0 <= s_normalized && s_normalized <= 1.0) { return geometry_msgs::build() - .x(start_point.x + (end_point.x - start_point.x) * s_normalized) - .y(start_point.y + (end_point.y - start_point.y) * s_normalized) - .z(start_point.z + (end_point.z - start_point.z) * s_normalized); - } - if (denormalize_s) { + .x(start_point.x + vector.x * s_normalized) + .y(start_point.y + vector.y * s_normalized) + .z(start_point.z + vector.z * s_normalized); + } else if (denormalize_s) { THROW_SIMULATION_ERROR( "Invalid S value is specified, while getting point on a line segment.", - "The range of s_normalized value should be in range [0,", getLength(), "].", - "But, your values are = ", s, " and length = ", getLength(), + "The range of s_normalized value should be in range [0,", length, "].", + "But, your values are = ", s, " and length = ", length, " This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } else { THROW_SIMULATION_ERROR( "Invalid S value is specified, while getting point on a line segment.", "The range of s_normalized value should be in range [0,1].", "But, your values are = ", s, - " and length = ", getLength(), + " and length = ", length, " This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } @@ -101,65 +111,55 @@ auto LineSegment::getPose(const double s, const bool denormalize_s, const bool f { return geometry_msgs::build() .position(getPoint(s, denormalize_s)) - .orientation([this, fill_pitch]() -> geometry_msgs::msg::Quaternion { - const auto tangent_vec = getVector(); - return math::geometry::convertEulerAngleToQuaternion( - geometry_msgs::build() - .x(0.0) - .y( - fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0) - .z(std::atan2(tangent_vec.y, tangent_vec.x))); - }()); + .orientation(math::geometry::convertEulerAngleToQuaternion( + geometry_msgs::build() + .x(0.0) + .y(fill_pitch ? std::atan2(-vector.z, std::hypot(vector.x, vector.y)) : 0.0) + .z(std::atan2(vector.y, vector.x)))); } /** * @brief Checking the intersection with 1 line segment and 1 point in 2D (x,y) coordinate. Ignore z axis. * @param point point you want to check intersection. * @return true Intersection detected. - * @return false Intersection does not detected. + * @return false Intersection not detected. */ auto LineSegment::isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool { - return getIntersection2DSValue(point, true) ? true : false; + if (isInBounds2D(point)) { + const double cross_product = + (point.y - start_point.y) * vector_2d.x - (point.x - start_point.x) * vector_2d.y; + constexpr double tolerance = std::numeric_limits::epsilon(); + return std::abs(cross_product) <= length_2d * tolerance; + } else { + return false; + } } auto LineSegment::getSValue( const geometry_msgs::msg::Pose & pose, double threshold_distance, bool denormalize_s) const -> std::optional { - return getIntersection2DSValue( + return get2DIntersectionSValue( LineSegment( math::geometry::transformPoint( - pose, geometry_msgs::build().x(0).y(threshold_distance).z(0)), + pose, + geometry_msgs::build().x(0.0).y(threshold_distance).z(0.0)), math::geometry::transformPoint( - pose, geometry_msgs::build().x(0).y(-threshold_distance).z(0))), + pose, + geometry_msgs::build().x(0.0).y(-threshold_distance).z(0.0))), denormalize_s); } /** * @brief Checking the intersection with 2 line segments in 2D (x,y) coordinate. Ignore z axis. - * @param l0 line segments you want to check intersection. + * @param line line segments you want to check intersection. * @return true Intersection detected. - * @return false Intersection does not detected. + * @return false Intersection not detected. */ -auto LineSegment::isIntersect2D(const LineSegment & l0) const -> bool +auto LineSegment::isIntersect2D(const LineSegment & line) const -> bool { - double s, t; - s = (l0.start_point.x - l0.end_point.x) * (start_point.y - l0.start_point.y) - - (l0.start_point.y - l0.end_point.y) * (start_point.x - l0.start_point.x); - t = (l0.start_point.x - l0.end_point.x) * (end_point.y - l0.start_point.y) - - (l0.start_point.y - l0.end_point.y) * (end_point.x - l0.start_point.x); - if (s * t > 0) { - return false; - } - s = (start_point.x - end_point.x) * (l0.start_point.y - start_point.y) - - (start_point.y - end_point.y) * (l0.start_point.x - start_point.x); - t = (start_point.x - end_point.x) * (l0.end_point.y - start_point.y) - - (start_point.y - end_point.y) * (l0.end_point.x - start_point.x); - if (s * t > 0) { - return false; - } - return true; + return math::geometry::isIntersect2D(*this, line); } /** @@ -167,35 +167,22 @@ auto LineSegment::isIntersect2D(const LineSegment & l0) const -> bool * @param point point of you want to find intersection. * @return std::optional */ -auto LineSegment::getIntersection2DSValue( +auto LineSegment::get2DIntersectionSValue( const geometry_msgs::msg::Point & point, const bool denormalize_s) const -> std::optional { - const auto get_s_normalized = [this](const auto & point) -> std::optional { - const auto get_s_from_x = [this](const auto & point) { - const auto s = (point.x - start_point.x) / (end_point.x - start_point.x); - return 0 <= s && s <= 1 ? s : std::optional(); - }; - const auto get_s_from_y = [this](const auto & point) { - const auto s = (point.y - start_point.y) / (end_point.y - start_point.y); - return 0 <= s && s <= 1 ? s : std::optional(); - }; - constexpr double epsilon = std::numeric_limits::epsilon(); - if (std::abs(end_point.x - start_point.x) <= epsilon) { - if (std::abs(end_point.y - start_point.y) <= epsilon) { - /// @note If start_point and end_point is a same point, checking the point is same as end_point or not. - return (std::abs(end_point.x - point.x) <= epsilon && - std::abs(end_point.y - point.y) <= epsilon) - ? std::optional(0) - : std::optional(); - } - /// @note If the line segment is parallel to y axis, calculate s value from y axis value. - return std::abs(point.x - start_point.x) <= epsilon ? get_s_from_y(point) - : std::optional(); - } - /// @note If the line segment is not parallel to x and y axis, calculate s value from x axis value. - return get_s_from_x(point); - }; - return denormalize_s ? denormalize(get_s_normalized(point)) : get_s_normalized(point); + /// @note This function checks for an SValue along the line. + /// The term "2D" in the function name specifically refers to the intersection point, not the SValue. + /// Therefore, the intersection is determined by disregarding the z-coordinate, hence the term "2D." + /// After finding the intersection, we calculate its position using a proportion. + /// Finally, we multiply this proportion by the actual 3D length to obtain the total SValue. + if (isIntersect2D(point)) { + const double proportion_2d = + std::hypot(point.x - start_point.x, point.y - start_point.y) / length_2d; + return denormalize_s ? std::make_optional(proportion_2d * length) + : std::make_optional(proportion_2d); + } else { + return std::nullopt; + } } /** @@ -204,33 +191,14 @@ auto LineSegment::getIntersection2DSValue( * @param denormalize_s If true, s value should be normalized in range [0,1]. If false, s value is not normalized. * @return std::optional */ -auto LineSegment::getIntersection2DSValue(const LineSegment & line, const bool denormalize_s) const +auto LineSegment::get2DIntersectionSValue(const LineSegment & line, const bool denormalize_s) const -> std::optional { - /// @note Hard coded parameter, this parameter describes the tolerance of the range of s value (-s_tolerance ~ 1.0 + s_tolerance) - constexpr double s_tolerance = 1e-10; - const auto get_s_normalized = [this](const auto & line) -> std::optional { - if (!isIntersect2D(line)) { - return std::optional(); - } - const double det = (start_point.x - end_point.x) * (line.end_point.y - line.start_point.y) - - (line.end_point.x - line.start_point.x) * (start_point.y - end_point.y); - const double s = - 1 - ((line.end_point.y - line.start_point.y) * (line.end_point.x - end_point.x) + - (line.start_point.x - line.end_point.x) * (line.end_point.y - end_point.y)) / - det; - if (std::isnan(s)) { - THROW_SIMULATION_ERROR( - "One line segment is on top of the other. So determinant is zero.", - "If this message was displayed, something completely unexpected happens.", - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); - } - return (-s_tolerance <= s && s <= 1 + s_tolerance) - ? std::optional(std::clamp(s, 0.0, 1.0)) - : std::optional(); - }; - return denormalize_s ? denormalize(get_s_normalized(line)) : get_s_normalized(line); + if (const auto point = getIntersection2D(line); point.has_value()) { + return get2DIntersectionSValue(point.value(), denormalize_s); + } else { + return std::nullopt; + } } /** @@ -241,23 +209,7 @@ auto LineSegment::getIntersection2DSValue(const LineSegment & line, const bool d auto LineSegment::getIntersection2D(const LineSegment & line) const -> std::optional { - const auto s = getIntersection2DSValue(line, false); - return s ? geometry_msgs::build() - .x(s.value() * start_point.x + (1.0 - s.value()) * end_point.x) - .y(s.value() * start_point.y + (1.0 - s.value()) * end_point.y) - .z(s.value() * start_point.z + (1.0 - s.value()) * end_point.z) - : std::optional(); -} - -auto LineSegment::getVector() const -> geometry_msgs::msg::Vector3 -{ - using math::geometry::operator-; - const auto result_pt = end_point - start_point; - auto result = geometry_msgs::msg::Vector3(); - result.x = result_pt.x; - result.y = result_pt.y; - result.z = result_pt.z; - return result; + return math::geometry::getIntersection2D(*this, line); } /** @@ -266,32 +218,20 @@ auto LineSegment::getVector() const -> geometry_msgs::msg::Vector3 */ auto LineSegment::getNormalVector() const -> geometry_msgs::msg::Vector3 { - geometry_msgs::msg::Vector3 tangent_vec = getVector(); - double theta = M_PI / 2.0; + const double theta = M_PI / 2.0; return geometry_msgs::build() - .x(tangent_vec.x * std::cos(theta) - tangent_vec.y * std::sin(theta)) - .y(tangent_vec.x * std::sin(theta) + tangent_vec.y * std::cos(theta)) - .z(0); + .x(vector.x * std::cos(theta) - vector.y * std::sin(theta)) + .y(vector.x * std::sin(theta) + vector.y * std::cos(theta)) + .z(0.0); } -auto LineSegment::get2DVector() const -> geometry_msgs::msg::Vector3 +auto LineSegment::get2DVectorSlope() const -> double { - return geometry_msgs::build() - .x(end_point.x - start_point.x) - .y(end_point.y - start_point.y) - .z(0); -} - -auto LineSegment::get2DLength() const -> double -{ - return std::hypot(end_point.x - start_point.x, end_point.y - start_point.y); -} - -auto LineSegment::getLength() const -> double { return hypot(end_point, start_point); } - -auto LineSegment::getSlope() const -> double -{ - return (end_point.y - start_point.y) / (end_point.x - start_point.x); + if (vector_2d.x <= std::numeric_limits::epsilon()) { + THROW_SIMULATION_ERROR("Slope of a vertical line is undefined"); + } else { + return vector_2d.y / vector_2d.x; + } } /** @@ -337,10 +277,11 @@ auto LineSegment::denormalize( const std::optional & s, const bool throw_error_on_out_of_range) const -> std::optional { - if (!throw_error_on_out_of_range && s && !(0 <= s.value() && s.value() <= 1)) { - return std::optional(); + if (!s.has_value() || (!throw_error_on_out_of_range && !(0.0 <= s.value() && s.value() <= 1.0))) { + return std::nullopt; + } else { + return std::make_optional(denormalize(s.value())); } - return s ? denormalize(s.value()) : std::optional(); } /** @@ -350,13 +291,14 @@ auto LineSegment::denormalize( */ auto LineSegment::denormalize(const double s) const -> double { - if (0 <= s && s <= 1) { - return s * getLength(); + if (0.0 <= s && s <= 1.0) { + return s * length; + } else { + THROW_SIMULATION_ERROR( + "Invalid normalized s value, s = ", s, ", S value should be in range [0,1].", + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); } - THROW_SIMULATION_ERROR( - "Invalid normalized s value, s = ", s, ", S value should be in range [0,1].", - "This message is not originally intended to be displayed, if you see it, please " - "contact the developer of traffic_simulator."); } LineSegment & LineSegment::operator=(const LineSegment &) { return *this; } @@ -386,5 +328,43 @@ auto getLineSegments( } } +/** + * @brief Checks if the given point lies within the bounding box of the line segment. + * @param point Points you want to test. + * @return + * - `true` if the points is within the bounds. + * - `false` otherwise. + */ +auto LineSegment::isInBounds2D(const geometry_msgs::msg::Point & point) const -> bool +{ + return ( + point.x >= std::min(start_point.x, end_point.x) && + point.x <= std::max(start_point.x, end_point.x) && + point.y >= std::min(start_point.y, end_point.y) && + point.y <= std::max(start_point.y, end_point.y)); +} + +/** + * @brief Determines the relative position of a point with respect to the line segment. + * + * This method computes the relative position of a given point with respect to the line segment defined by + * `start_point` and `end_point` using a cross product. The result indicates on which side of the line + * segment the point lies. + * + * @param point The point to be evaluated. + * @return + * - `1` if the point is to the left of the line segment (when moving from `start_point` to `end_point`). + * - `-1` if the point is to the right of the line segment. + * - `0` if the point is collinear with the line segment (i.e., lies exactly on the line defined by + * `start_point` and `end_point`). + */ +auto LineSegment::relativePointPosition2D(const geometry_msgs::msg::Point & point) const -> int +{ + constexpr double tolerance = std::numeric_limits::epsilon(); + const double determinant = + vector_2d.y * (point.x - end_point.x) - vector_2d.x * (point.y - end_point.y); + return static_cast(determinant > tolerance) - static_cast(determinant < -tolerance); +} + } // namespace geometry } // namespace math diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index 523b6f28099..01dafbf7594 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -139,7 +139,7 @@ CatmullRomSpline::CatmullRomSpline(const std::vector break; /// @note In this case, spline is interpreted as line segment. case 2: - total_length_ = line_segments_[0].getLength(); + total_length_ = line_segments_[0].length; break; /// @note In this case, spline is interpreted as curve. default: @@ -316,7 +316,7 @@ auto CatmullRomSpline::getCollisionPointsIn2D( "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } - if (const auto s = line_segments_[0].getIntersection2DSValue(line, true)) { + if (const auto s = line_segments_[0].get2DIntersectionSValue(line, true)) { s_value_candidates.insert(s.value()); } } @@ -628,7 +628,7 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs:: "contact the developer of traffic_simulator."); } if (0 <= s && s <= getLength()) { - return line_segments_[0].getVector(); + return line_segments_[0].vector; } THROW_SIMULATION_ERROR( "Invalid S value is specified, while getting tangent vector.", diff --git a/common/math/geometry/test/CMakeLists.txt b/common/math/geometry/test/CMakeLists.txt index dfd0a09460b..eed4ab3431c 100644 --- a/common/math/geometry/test/CMakeLists.txt +++ b/common/math/geometry/test/CMakeLists.txt @@ -1,26 +1,15 @@ -add_subdirectory(vector3) +add_subdirectory(src/intersection) +add_subdirectory(src/polygon) +add_subdirectory(src/quaternion) +add_subdirectory(src/solver) +add_subdirectory(src/spline) +add_subdirectory(src/vector3) -ament_add_gtest(test_bounding_box test_bounding_box.cpp) -ament_add_gtest(test_catmull_rom_spline test_catmull_rom_spline.cpp) -ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp) -ament_add_gtest(test_collision test_collision.cpp) -ament_add_gtest(test_distance test_distance.cpp) -ament_add_gtest(test_hermite_curve test_hermite_curve.cpp) -ament_add_gtest(test_polygon test_polygon.cpp) -ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp) -ament_add_gtest(test_transform test_transform.cpp) -ament_add_gtest(test_intersection test_intersection.cpp) -ament_add_gtest(test_line_segment test_line_segment.cpp) -ament_add_gtest(test_quaternion test_quaternion.cpp) -target_link_libraries(test_bounding_box geometry) -target_link_libraries(test_catmull_rom_spline geometry) -target_link_libraries(test_catmull_rom_subspline geometry) -target_link_libraries(test_collision geometry) +ament_add_gtest(test_distance src/test_distance.cpp) target_link_libraries(test_distance geometry) -target_link_libraries(test_hermite_curve geometry) -target_link_libraries(test_polygon geometry) -target_link_libraries(test_polynomial_solver geometry) + +ament_add_gtest(test_bounding_box src/test_bounding_box.cpp) +target_link_libraries(test_bounding_box geometry) + +ament_add_gtest(test_transform src/test_transform.cpp) target_link_libraries(test_transform geometry) -target_link_libraries(test_intersection geometry) -target_link_libraries(test_line_segment geometry) -target_link_libraries(test_quaternion geometry) diff --git a/common/math/geometry/test/expect_eq_macros.hpp b/common/math/geometry/test/src/expect_eq_macros.hpp similarity index 100% rename from common/math/geometry/test/expect_eq_macros.hpp rename to common/math/geometry/test/src/expect_eq_macros.hpp diff --git a/common/math/geometry/test/src/intersection/CMakeLists.txt b/common/math/geometry/test/src/intersection/CMakeLists.txt new file mode 100644 index 00000000000..ea171973a2c --- /dev/null +++ b/common/math/geometry/test/src/intersection/CMakeLists.txt @@ -0,0 +1,5 @@ +ament_add_gtest(test_collision test_collision.cpp) +target_link_libraries(test_collision geometry) + +ament_add_gtest(test_intersection test_intersection.cpp) +target_link_libraries(test_intersection geometry) diff --git a/common/math/geometry/test/test_collision.cpp b/common/math/geometry/test/src/intersection/test_collision.cpp similarity index 99% rename from common/math/geometry/test/test_collision.cpp rename to common/math/geometry/test/src/intersection/test_collision.cpp index cf60a70eba2..a32622d4f0c 100644 --- a/common/math/geometry/test/test_collision.cpp +++ b/common/math/geometry/test/src/intersection/test_collision.cpp @@ -17,7 +17,7 @@ #include #include -#include "test_utils.hpp" +#include "../test_utils.hpp" TEST(Collision, DifferentHeight) { diff --git a/common/math/geometry/test/test_intersection.cpp b/common/math/geometry/test/src/intersection/test_intersection.cpp similarity index 91% rename from common/math/geometry/test/test_intersection.cpp rename to common/math/geometry/test/src/intersection/test_intersection.cpp index 4ee95a08cd9..f21879f6555 100644 --- a/common/math/geometry/test/test_intersection.cpp +++ b/common/math/geometry/test/src/intersection/test_intersection.cpp @@ -16,9 +16,10 @@ #include #include +#include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" TEST(Intersection, isIntersect2DDisjoint) { @@ -108,9 +109,7 @@ TEST(Intersection, getIntersection2DIntersectVector) TEST(Intersection, getIntersection2DIdentical) { math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - auto ans = math::geometry::getIntersection2D(line, line); - EXPECT_TRUE(ans); - EXPECT_POINT_NAN(ans.value()); + EXPECT_THROW(math::geometry::getIntersection2D(line, line), common::SimulationError); } TEST(Intersection, getIntersection2DIdenticalVector) @@ -121,14 +120,7 @@ TEST(Intersection, getIntersection2DIdenticalVector) lines.push_back(line); lines.push_back(line); - auto ans = math::geometry::getIntersection2D(lines); - EXPECT_EQ(ans.size(), size_t(6)); - EXPECT_POINT_NAN(ans[0]); - EXPECT_POINT_NAN(ans[1]); - EXPECT_POINT_NAN(ans[2]); - EXPECT_POINT_NAN(ans[3]); - EXPECT_POINT_NAN(ans[4]); - EXPECT_POINT_NAN(ans[5]); + EXPECT_THROW(math::geometry::getIntersection2D(lines), common::SimulationError); } TEST(Intersection, getIntersection2DEmptyVector) diff --git a/common/math/geometry/test/src/polygon/CMakeLists.txt b/common/math/geometry/test/src/polygon/CMakeLists.txt new file mode 100644 index 00000000000..8a2eae13418 --- /dev/null +++ b/common/math/geometry/test/src/polygon/CMakeLists.txt @@ -0,0 +1,5 @@ +ament_add_gtest(test_line_segment test_line_segment.cpp) +target_link_libraries(test_line_segment geometry) + +ament_add_gtest(test_polygon test_polygon.cpp) +target_link_libraries(test_polygon geometry) diff --git a/common/math/geometry/test/src/polygon/test_line_segment.cpp b/common/math/geometry/test/src/polygon/test_line_segment.cpp new file mode 100644 index 00000000000..354e9d71584 --- /dev/null +++ b/common/math/geometry/test/src/polygon/test_line_segment.cpp @@ -0,0 +1,785 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include + +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" + +TEST(LineSegment, initializeDifferentPoints) +{ + geometry_msgs::msg::Point point0 = makePoint(0.0, 0.0); + geometry_msgs::msg::Point point1 = makePoint(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point0, point1)); + const math::geometry::LineSegment line_segment(point0, point1); + EXPECT_POINT_EQ(line_segment.start_point, point0); + EXPECT_POINT_EQ(line_segment.end_point, point1); +} + +TEST(LineSegment, initializeIdenticalPoints) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, point)); + const math::geometry::LineSegment line_segment(point, point); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, point); +} + +TEST(LineSegment, initializeVector) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 1.0)); + const math::geometry::LineSegment line_segment(point, vec, 1.0); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, makePoint(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0)); +} + +TEST(LineSegment, initializeVectorZero) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 0.0); + EXPECT_THROW( + const math::geometry::LineSegment line_segment(point, vec, 1.0), common::SimulationError); +} + +TEST(LineSegment, initializeVectorZeroLength) +{ + geometry_msgs::msg::Point point = makePoint(0.0, 0.0); + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); + EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 0.0)); + const math::geometry::LineSegment line_segment(point, vec, 0.0); + EXPECT_POINT_EQ(line_segment.start_point, point); + EXPECT_POINT_EQ(line_segment.end_point, point); +} + +/** + * @note Test error throwing when s is out of bounds. + */ +TEST(LineSegment, getPoint_outOfBounds_denormalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_THROW(line.getPoint(7.0, true), common::SimulationError); + EXPECT_THROW(line.getPoint(-1.0, true), common::SimulationError); +} + +/** + * @note Test error throwing when normalized s is out of bounds. + */ +TEST(LineSegment, getPoint_outOfBounds_normalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_THROW(line.getPoint(1.1, false), common::SimulationError); + EXPECT_THROW(line.getPoint(-0.1, false), common::SimulationError); +} + +/** + * @note Test calculation correctness when s is out of bounds. + */ +TEST(LineSegment, getPoint_inside_denormalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_POINT_EQ(line.getPoint(0.0, true), makePoint(-1.0, -2.0, 1.0)); + EXPECT_POINT_EQ(line.getPoint(3.0, true), makePoint(1.0, 0.0, 0.0)); + EXPECT_POINT_EQ(line.getPoint(6.0, true), makePoint(3.0, 2.0, -1.0)); +} + +/** + * @note Test calculation correctness when normalized s is out of bounds. + */ +TEST(LineSegment, getPoint_inside_normalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 1.0), makePoint(3.0, 2.0, -1.0)); + ASSERT_DOUBLE_EQ(line.length, 6.0); + + EXPECT_POINT_EQ(line.getPoint(0.0, false), makePoint(-1.0, -2.0, 1.0)); + EXPECT_POINT_EQ(line.getPoint(0.5, false), makePoint(1.0, 0.0, 0.0)); + EXPECT_POINT_EQ(line.getPoint(1.0, false), makePoint(3.0, 2.0, -1.0)); +} + +/** + * @note Test calculation correctness with denormalized s. + */ +TEST(LineSegment, getPose_denormalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + const double length = 4.0 * std::sqrt(3.0); + ASSERT_DOUBLE_EQ(line.length, length); + + EXPECT_POSE_EQ( + line.getPose(0.0 * length, true, false), + makePose( + -1.0, -2.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(0.5 * length, true, false), + makePose( + 1.0, 0.0, 2.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(1.0 * length, true, false), + makePose( + 3.0, 2.0, 4.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); +} + +/** + * @note Test calculation correctness with normalized s. + */ +TEST(LineSegment, getPose_normalized) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + const double length = 4.0 * std::sqrt(3.0); + ASSERT_DOUBLE_EQ(line.length, length); + + EXPECT_POSE_EQ( + line.getPose(0.0, false, false), + makePose( + -1.0, -2.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(0.5, false, false), + makePose( + 1.0, 0.0, 2.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(1.0, false, false), + makePose( + 3.0, 2.0, 4.0, math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4)))); +} + +/** + * @note Test pitch calculation correctness with fill_pitch = true. + */ +TEST(LineSegment, getPose_pitch) +{ + const auto line = math::geometry::LineSegment( + makePoint(-1.0, -2.0, 0.0 * std::sqrt(2.0)), makePoint(3.0, 2.0, 4.0 * std::sqrt(2.0))); + const double length = 8.0; + ASSERT_DOUBLE_EQ(line.length, length); + + EXPECT_POSE_EQ( + line.getPose(0.0 * length, true, true), + makePose( + -1.0, -2.0, 0.0 * std::sqrt(2.0), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, -M_PI_4, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(0.5 * length, true, true), + makePose( + 1.0, 0.0, 2.0 * std::sqrt(2.0), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, -M_PI_4, M_PI_4)))); + + EXPECT_POSE_EQ( + line.getPose(1.0 * length, true, true), + makePose( + 3.0, 2.0, 4.0 * std::sqrt(2.0), + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, -M_PI_4, M_PI_4)))); +} + +TEST(LineSegment, isIntersect2DDisjoint) +{ + const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(line0.isIntersect2D(line1)); +} + +TEST(LineSegment, isIntersect2DIntersect) +{ + const math::geometry::LineSegment line0(makePoint(1.0, 1.0), makePoint(2.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makeVector(1.0, 1.0), 3.0); + EXPECT_TRUE(line0.isIntersect2D(line1)); +} + +TEST(LineSegment, isIntersect2DIdentical) +{ + const math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + EXPECT_TRUE(line.isIntersect2D(line)); +} + +/** + * @note Test function behavior with two disjoint, collinear lines. + */ +TEST(LineSegment, isIntersect2D_collinear) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + + EXPECT_FALSE(line.isIntersect2D( + math::geometry::LineSegment(makePoint(3.0, 5.0, 0.0), makePoint(5.0, 7.0, 0.0)))); + EXPECT_FALSE(line.isIntersect2D( + math::geometry::LineSegment(makePoint(-3.0, -1.0, 0.0), makePoint(-2.0, 0.0, 0.0)))); +} + +/** + * @note Test function behavior with a point on the line. + */ +TEST(LineSegment, isIntersect2D_pointInside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_TRUE(line.isIntersect2D(makePoint(0.0, -1.0, 1.0))); + EXPECT_TRUE(line.isIntersect2D(makePoint(0.0, -1.0, 0.0))); +} + +/** + * @note Test function behavior with a point outside of the line. + */ +TEST(LineSegment, isIntersect2D_pointOutside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_FALSE(line.isIntersect2D(makePoint(-1.0, -1.0, 1.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(0.0, 89.0, 97.0))); + + EXPECT_FALSE(line.isIntersect2D(makePoint(-3.0, 0.0, 0.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(4.0, 0.0, 0.0))); +} + +/** + * @note Test function behavior with a point that is collinear and external to the line. + */ +TEST(LineSegment, isIntersect2D_pointCollinear) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_FALSE(line.isIntersect2D(makePoint(-2.0, -3.0, -1.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(-2.0, -3.0, 0.0))); + + EXPECT_FALSE(line.isIntersect2D(makePoint(4.0, 3.0, 5.0))); + EXPECT_FALSE(line.isIntersect2D(makePoint(4.0, 3.0, 0.0))); +} + +/** + * @note Test function behavior with a point on an end of the line. + */ +TEST(LineSegment, isIntersect2D_pointOnEnd) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(3.0, 2.0, 4.0)); + + EXPECT_TRUE(line.isIntersect2D(makePoint(-1.0, -2.0, 0.0))); + EXPECT_TRUE(line.isIntersect2D(makePoint(-1.0, -2.0, 7.0))); + + EXPECT_TRUE(line.isIntersect2D(makePoint(3.0, 2.0, 4.0))); + EXPECT_TRUE(line.isIntersect2D(makePoint(3.0, 2.0, -5.0))); +} + +/** + * @note Test result correctness with a line intersecting with a vertical line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_vertical) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(-1.0, 2.0, 0.0)); + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 1.0, 0.0)), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 1.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.25); + } +} + +/** + * @note Test result correctness with a line intersecting with a horizontal line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_horizontal) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 2.0, 0.0), makePoint(1.0, 2.0, 0.0)); + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-1.0, 0.0, 0.0), makePoint(1.0, 4.0, 0.0)), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-1.0, 0.0, 0.0), makePoint(1.0, 4.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.5); + } +} + +/** + * @note Test result correctness with lines intersecting at the start and end of a line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_bounds) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, 2.0, 0.0), makePoint(0.0, 0.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.0); + } + { + const auto s_value = line.get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(1.0, 3.0, 0.0), makePoint(2.0, 2.0, 0.0)), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } +} + +/** + * @note Test result correctness with a line outside of the line. + */ +TEST(LineSegment, get2DIntersectionSValue_line_outside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + + EXPECT_FALSE( + line + .get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(-2.0, 1.0, 0.0), makePoint(0.0, 0.0, 0.0)), false) + .has_value()); + + EXPECT_FALSE( + line + .get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(1.0, 4.0, 0.0), makePoint(2.0, 2.0, 0.0)), false) + .has_value()); +} + +/** + * @note Test result correctness when lines are collinear. + */ +TEST(LineSegment, get2DIntersectionSValue_line_collinear) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 1.0, 0.0), makePoint(1.0, 3.0, 0.0)); + + EXPECT_THROW(line.get2DIntersectionSValue(line, false), common::SimulationError); + + EXPECT_FALSE( + line + .get2DIntersectionSValue( + math::geometry::LineSegment(makePoint(3.0, 5.0, 0.0), makePoint(5.0, 7.0, 0.0)), false) + .has_value()); +} + +/** + * @note Test result correctness with a point inside a vertical line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_vertical) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, -2.0, 0.0), makePoint(-1.0, 2.0, 0.0)); + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-1.0, 0.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 2.0); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-1.0, 0.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.5); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-101.0, 0.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(103.0, 0.0, 0.0), false).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-1.0, -107.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-1.0, 109.0, 0.0), false).has_value()); + } +} + +/** + * @note Test result correctness with a point inside a horizontal line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_horizontal) +{ + const auto line = + math::geometry::LineSegment(makePoint(-1.0, 2.0, 0.0), makePoint(1.0, 2.0, 0.0)); + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(0.0, 2.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(0.0, 2.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.5); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-113.0, 2.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(127.0, 2.0, 0.0), false).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(0.0, -131.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(0.0, 137.0, 0.0), false).has_value()); + } +} + +/** + * @note Test result correctness with points at the start and end of a line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_bounds) +{ + const auto line = + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 4.0, 0.0)); + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-2.0, -2.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.0); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(-2.0, -2.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 0.0); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-139.0, -2.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(149.0, -2.0, 0.0), false).has_value()); + } + + { + const auto s_value = line.get2DIntersectionSValue(makePoint(1.0, 4.0, 0.0), true); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 3.0 * std::sqrt(5)); + } + { + const auto s_value = line.get2DIntersectionSValue(makePoint(1.0, 4.0, 0.0), false); + ASSERT_TRUE(s_value.has_value()); + EXPECT_DOUBLE_EQ(s_value.value(), 1.0); + } + { + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-151.0, 4.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(157.0, 4.0, 0.0), false).has_value()); + } +} + +/** + * @note Test result correctness with a point outside of the line. + */ +TEST(LineSegment, get2DIntersectionSValue_point_outside) +{ + const auto line = + math::geometry::LineSegment(makePoint(-2.0, -2.0, 0.0), makePoint(1.0, 4.0, 0.0)); + + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-3.0, 1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-1.0, 1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(0.0, 1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(2.0, 1.0, 0.0), true).has_value()); + + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, -5.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, -1.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, 3.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-0.5, 7.0, 0.0), true).has_value()); + + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(7.0, 7.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(7.0, -7.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-7.0, 7.0, 0.0), true).has_value()); + EXPECT_FALSE(line.get2DIntersectionSValue(makePoint(-7.0, -7.0, 0.0), true).has_value()); +} + +TEST(LineSegment, getIntersection2DDisjoint) +{ + const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(line0.getIntersection2D(line1)); +} + +/** + * @note Test function behavior with two intersecting lines. + */ +TEST(LineSegment, getIntersection2DIntersect) +{ + const auto line0 = math::geometry::LineSegment(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + const auto line1 = math::geometry::LineSegment(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + + const auto p0 = line0.getIntersection2D(line1); + const auto p1 = line1.getIntersection2D(line0); + + ASSERT_TRUE(p0.has_value()); + ASSERT_TRUE(p1.has_value()); + EXPECT_POINT_EQ(p0.value(), makePoint(0.5, 0.5)); + EXPECT_POINT_EQ(p1.value(), makePoint(0.5, 0.5)); +} + +/** + * @note Test function behavior with two identical lines. + */ +TEST(LineSegment, getIntersection2DIdentical) +{ + const auto line = math::geometry::LineSegment(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + + EXPECT_THROW(line.getIntersection2D(line), common::SimulationError); +} + +TEST(LineSegment, getSValue) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, false); + EXPECT_TRUE(s); + if (s) { + EXPECT_DOUBLE_EQ(s.value(), 2.0 / 3.0); + } +} + +TEST(LineSegment, getSValue_denormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, true); + EXPECT_TRUE(s); + if (s) { + EXPECT_DOUBLE_EQ(s.value(), std::hypot(2.0, 2.0, 2.0)); + } +} + +TEST(LineSegment, getSValue_outOfRange) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, false); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_outOfRangeDenormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); + const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, true); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_parallel) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); + const auto s = line.getSValue( + makePose( + 1.0, 0.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + 1000.0, false); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getSValue_parallelDenormalize) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); + const auto s = line.getSValue( + makePose( + 1.0, 0.0, 0.0, + math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), + 1000.0, true); + EXPECT_FALSE(s); +} + +TEST(LineSegment, getVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.vector, makeVector(1.0, 1.0, 1.0)); +} + +TEST(LineSegment, getVectorZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.vector, makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, getNormalVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(-1.0, 1.0, 0.0)); +} + +TEST(LineSegment, getNormalVector_zeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, get2DVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_VECTOR3_EQ(line.vector_2d, makeVector(1.0, 1.0, 0.0)); +} + +TEST(LineSegment, get2DVectorZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_VECTOR3_EQ(line.vector_2d, makeVector(0.0, 0.0, 0.0)); +} + +TEST(LineSegment, getLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.length, std::sqrt(3.0)); +} + +TEST(LineSegment, getLengthZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_DOUBLE_EQ(line.length, 0.0); +} + +TEST(LineSegment, get2DLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.length_2d, std::sqrt(2.0)); +} + +TEST(LineSegment, get2DLengthZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_DOUBLE_EQ(line.length_2d, 0.0); +} + +TEST(LineSegment, get2DVectorSlope) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 3.0, 4.0)); + EXPECT_DOUBLE_EQ(line.get2DVectorSlope(), 0.5); +} + +TEST(LineSegment, get2DVectorSlopeZeroLength) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); + EXPECT_THROW(line.get2DVectorSlope(), common::SimulationError); +} + +TEST(LineSegment, getSquaredDistanceIn2D) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_DOUBLE_EQ(line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), 0.5, false), 8.0); +} + +TEST(LineSegment, getSquaredDistanceIn2D_denormalize) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_DOUBLE_EQ( + line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), 8.0); +} + +TEST(LineSegment, getSquaredDistanceVector) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_VECTOR3_EQ( + line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), 0.5, false), + makeVector(-2.0, -2.0, -2.0)); +} + +TEST(LineSegment, getSquaredDistanceVector_denormalize) +{ + const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); + EXPECT_VECTOR3_EQ( + line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), + makeVector(-2.0, -2.0, -2.0)); +} + +TEST(LineSegment, getLineSegments) +{ + const std::vector points{ + makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), + makePoint(4.0, 5.0, 6.0)}; + const std::vector lines = + math::geometry::getLineSegments(points, false); + EXPECT_EQ(lines.size(), size_t(3)); + EXPECT_POINT_EQ(lines[0].start_point, points[0]); + EXPECT_POINT_EQ(lines[0].end_point, points[1]); + EXPECT_POINT_EQ(lines[1].start_point, points[1]); + EXPECT_POINT_EQ(lines[1].end_point, points[2]); + EXPECT_POINT_EQ(lines[2].start_point, points[2]); + EXPECT_POINT_EQ(lines[2].end_point, points[3]); +} + +TEST(LineSegment, getLineSegments_closeStartEnd) +{ + const std::vector points{ + makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), + makePoint(4.0, 5.0, 6.0)}; + const std::vector lines = + math::geometry::getLineSegments(points, true); + EXPECT_EQ(lines.size(), size_t(4)); + EXPECT_POINT_EQ(lines[0].start_point, points[0]); + EXPECT_POINT_EQ(lines[0].end_point, points[1]); + EXPECT_POINT_EQ(lines[1].start_point, points[1]); + EXPECT_POINT_EQ(lines[1].end_point, points[2]); + EXPECT_POINT_EQ(lines[2].start_point, points[2]); + EXPECT_POINT_EQ(lines[2].end_point, points[3]); + EXPECT_POINT_EQ(lines[3].start_point, points[3]); + EXPECT_POINT_EQ(lines[3].end_point, points[0]); +} + +TEST(LineSegment, getLineSegmentsVectorEmpty) +{ + const std::vector points; + const std::vector lines = math::geometry::getLineSegments(points); + EXPECT_EQ(lines.size(), size_t(0)); +} + +TEST(LineSegment, getLineSegmentsVectorIdentical) +{ + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + const std::vector points{point, point, point, point}; + const std::vector lines = + math::geometry::getLineSegments(points, false); + EXPECT_EQ(lines.size(), size_t(3)); + EXPECT_POINT_EQ(lines[0].start_point, point); + EXPECT_POINT_EQ(lines[0].end_point, point); + EXPECT_POINT_EQ(lines[1].start_point, point); + EXPECT_POINT_EQ(lines[1].end_point, point); + EXPECT_POINT_EQ(lines[2].start_point, point); + EXPECT_POINT_EQ(lines[2].end_point, point); +} + +TEST(LineSegment, getLineSegmentsVectorIdentical_closeStartEnd) +{ + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + const std::vector points{point, point, point, point}; + const std::vector lines = + math::geometry::getLineSegments(points, true); + EXPECT_EQ(lines.size(), size_t(4)); + EXPECT_POINT_EQ(lines[0].start_point, point); + EXPECT_POINT_EQ(lines[0].end_point, point); + EXPECT_POINT_EQ(lines[1].start_point, point); + EXPECT_POINT_EQ(lines[1].end_point, point); + EXPECT_POINT_EQ(lines[2].start_point, point); + EXPECT_POINT_EQ(lines[2].end_point, point); + EXPECT_POINT_EQ(lines[3].start_point, point); + EXPECT_POINT_EQ(lines[3].end_point, point); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_polygon.cpp b/common/math/geometry/test/src/polygon/test_polygon.cpp similarity index 98% rename from common/math/geometry/test/test_polygon.cpp rename to common/math/geometry/test/src/polygon/test_polygon.cpp index 8ec0ea97ad9..e7278fa6566 100644 --- a/common/math/geometry/test/test_polygon.cpp +++ b/common/math/geometry/test/src/polygon/test_polygon.cpp @@ -17,8 +17,8 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" TEST(Polygon, filterByAxis) { diff --git a/common/math/geometry/test/src/quaternion/CMakeLists.txt b/common/math/geometry/test/src/quaternion/CMakeLists.txt new file mode 100644 index 00000000000..2ea2aaf3d91 --- /dev/null +++ b/common/math/geometry/test/src/quaternion/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_quaternion test_quaternion.cpp) +target_link_libraries(test_quaternion geometry) diff --git a/common/math/geometry/test/test_quaternion.cpp b/common/math/geometry/test/src/quaternion/test_quaternion.cpp similarity index 59% rename from common/math/geometry/test/test_quaternion.cpp rename to common/math/geometry/test/src/quaternion/test_quaternion.cpp index 7dfd1285664..0b31df60805 100644 --- a/common/math/geometry/test/test_quaternion.cpp +++ b/common/math/geometry/test/src/quaternion/test_quaternion.cpp @@ -18,47 +18,59 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-6; -TEST(Quaternion, testCase1) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_addition) { using math::geometry::operator+; - auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto ans = q1 + q2; + const auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto ans = q1 + q2; EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 2, 0, 2)) } -TEST(Quaternion, testCase2) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_subtraction) { using math::geometry::operator-; - auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto ans = q1 - q2; + const auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto ans = q1 - q2; EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 0, 0, 0)) } -TEST(Quaternion, testCase3) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_multiplication) { using math::geometry::operator*; - auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto ans = q1 * q2; + const auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto ans = q1 * q2; EXPECT_QUATERNION_EQ(ans, math::geometry::makeQuaternion(0, 2, 0, 0)) } -TEST(Quaternion, testCase4) +/** + * @note Test result correctness with a basic example. + */ +TEST(Quaternion, operator_additionAssignment) { using math::geometry::operator+=; auto q1 = math::geometry::makeQuaternion(0, 1, 0, 1); - auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); + const auto q2 = math::geometry::makeQuaternion(0, 1, 0, 1); q1 += q2; EXPECT_QUATERNION_EQ(q1, math::geometry::makeQuaternion(0, 2, 0, 2)) } diff --git a/common/math/geometry/test/src/solver/CMakeLists.txt b/common/math/geometry/test/src/solver/CMakeLists.txt new file mode 100644 index 00000000000..0716910f184 --- /dev/null +++ b/common/math/geometry/test/src/solver/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_polynomial_solver test_polynomial_solver.cpp) +target_link_libraries(test_polynomial_solver geometry) diff --git a/common/math/geometry/test/test_polynomial_solver.cpp b/common/math/geometry/test/src/solver/test_polynomial_solver.cpp similarity index 100% rename from common/math/geometry/test/test_polynomial_solver.cpp rename to common/math/geometry/test/src/solver/test_polynomial_solver.cpp diff --git a/common/math/geometry/test/src/spline/CMakeLists.txt b/common/math/geometry/test/src/spline/CMakeLists.txt new file mode 100644 index 00000000000..d6a2793f80d --- /dev/null +++ b/common/math/geometry/test/src/spline/CMakeLists.txt @@ -0,0 +1,8 @@ +ament_add_gtest(test_catmull_rom_spline test_catmull_rom_spline.cpp) +target_link_libraries(test_catmull_rom_spline geometry) + +ament_add_gtest(test_catmull_rom_subspline test_catmull_rom_subspline.cpp) +target_link_libraries(test_catmull_rom_subspline geometry) + +ament_add_gtest(test_hermite_curve test_hermite_curve.cpp) +target_link_libraries(test_hermite_curve geometry) diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/src/spline/test_catmull_rom_spline.cpp similarity index 99% rename from common/math/geometry/test/test_catmull_rom_spline.cpp rename to common/math/geometry/test/src/spline/test_catmull_rom_spline.cpp index b29aba00130..68ad3855eca 100644 --- a/common/math/geometry/test/test_catmull_rom_spline.cpp +++ b/common/math/geometry/test/src/spline/test_catmull_rom_spline.cpp @@ -18,8 +18,8 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-6; @@ -70,7 +70,7 @@ void addOffset(geometry_msgs::msg::Point & point, const double offset, const dou /// @brief Testing the `CatmullRomSpline::getCollisionPointIn2D` function works valid. /// In this test case, number of the control points of the catmull-rom spline (variable name `spline`) is 2, so the shape of the value `spline` is line segment. -TEST(CatmullRomSpline, GetCollisionWith2ControlPoints) +TEST(CatmullRomSpline, getCollisionPointIn2D_2ControlPoints) { /// @note The `spline` has control points p0 and p1. Control point p0 is point (x,y,z) = (0,0,0) and control point p1 is point (x,y,z) = (1,0,0) in the cartesian coordinate system. // [Snippet_construct_spline] @@ -149,7 +149,7 @@ TEST(CatmullRomSpline, GetCollisionWith2ControlPoints) /// @brief Testing the `CatmullRomSpline::getCollisionPointIn2D` function works valid /// In this test case, number of the control points of the catmull-rom spline (variable name `spline`) is 1, so the shape of the value `spline` is single point. -TEST(CatmullRomSpline, GetCollisionWith1ControlPoint) +TEST(CatmullRomSpline, getCollisionPointIn2D_1ControlPoint) { /// @note The variable `spline` has control point with point (x,y,z) = (0,1,0) in the cartesian coordinate system. So, `spline` is same as point (x,y,z) = (0,1,0). auto spline = math::geometry::CatmullRomSpline( diff --git a/common/math/geometry/test/test_catmull_rom_subspline.cpp b/common/math/geometry/test/src/spline/test_catmull_rom_subspline.cpp similarity index 98% rename from common/math/geometry/test/test_catmull_rom_subspline.cpp rename to common/math/geometry/test/src/spline/test_catmull_rom_subspline.cpp index 567d1bf089e..10b2b56124c 100644 --- a/common/math/geometry/test/test_catmull_rom_subspline.cpp +++ b/common/math/geometry/test/src/spline/test_catmull_rom_subspline.cpp @@ -17,8 +17,8 @@ #include #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-6; diff --git a/common/math/geometry/test/test_hermite_curve.cpp b/common/math/geometry/test/src/spline/test_hermite_curve.cpp similarity index 94% rename from common/math/geometry/test/test_hermite_curve.cpp rename to common/math/geometry/test/src/spline/test_hermite_curve.cpp index 9efbccef633..96eb39fd2ec 100644 --- a/common/math/geometry/test/test_hermite_curve.cpp +++ b/common/math/geometry/test/src/spline/test_hermite_curve.cpp @@ -16,8 +16,8 @@ #include -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" constexpr double EPS = 1e-3; @@ -320,7 +320,10 @@ TEST(HermiteCurveTest, getSValue) EXPECT_NEAR(s2.value(), 1.0, EPS); } -TEST(HermiteCurveTest, getSValueAutoscale) +/** + * @note Test function correctness with parameter denormalize_s = true. + */ +TEST(HermiteCurveTest, getSValueDenormalized) { const auto curve = makeLine2(); @@ -503,7 +506,10 @@ TEST(HermiteCurveTest, getTangentVector4) EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); } -TEST(HermiteCurveTest, getTangentVectorAutoscale1) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized1) { //p(0,0) v(1,0)-> p(1,1) v(0,1) const auto curve = makeCurve1(); constexpr double eps = 0.1; @@ -513,7 +519,10 @@ TEST(HermiteCurveTest, getTangentVectorAutoscale1) EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getTangentVectorAutoscale2) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized2) { //p(0,0) v(1,0)-> p(1,-1) v(0,-1) const auto curve = makeCurve2(); constexpr double eps = 0.1; @@ -523,7 +532,10 @@ TEST(HermiteCurveTest, getTangentVectorAutoscale2) EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getTangentVectorAutoscale3) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized3) { //p(1,1) v(0,-1)-> p(0,0) v(-1,0) const auto curve = makeCurve3(); constexpr double eps = 0.1; @@ -533,7 +545,10 @@ TEST(HermiteCurveTest, getTangentVectorAutoscale3) EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getTangentVectorAutoscale4) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getTangentVectorDenormalized4) { //p(1,-1) v(0,1)-> p(0,0) v(-1,0) const auto curve = makeCurve4(); constexpr double eps = 0.1; @@ -579,7 +594,10 @@ TEST(HermiteCurveTest, getNormalVector4) EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); } -TEST(HermiteCurveTest, getNormalVectorAutoscale1) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized1) { //p(0,0) v(1,0)-> p(1,1) v(0,1) const auto curve = makeCurve1(); constexpr double eps = 0.1; @@ -589,7 +607,10 @@ TEST(HermiteCurveTest, getNormalVectorAutoscale1) EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getNormalVectorAutoscale2) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized2) { //p(0,0) v(1,0)-> p(1,-1) v(0,-1) const auto curve = makeCurve2(); constexpr double eps = 0.1; @@ -599,7 +620,10 @@ TEST(HermiteCurveTest, getNormalVectorAutoscale2) EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getNormalVectorAutoscale3) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized3) { //p(1,1) v(0,-1)-> p(0,0) v(-1,0) const auto curve = makeCurve3(); constexpr double eps = 0.1; @@ -609,7 +633,10 @@ TEST(HermiteCurveTest, getNormalVectorAutoscale3) EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); } -TEST(HermiteCurveTest, getNormalVectorAutoscale4) +/** + * @note Test function correctness with parameter denormalize_s = true with different curves. + */ +TEST(HermiteCurveTest, getNormalVectorDenormalized4) { //p(1,-1) v(0,1)-> p(0,0) v(-1,0) const auto curve = makeCurve4(); constexpr double eps = 0.1; diff --git a/common/math/geometry/test/test_bounding_box.cpp b/common/math/geometry/test/src/test_bounding_box.cpp similarity index 55% rename from common/math/geometry/test/test_bounding_box.cpp rename to common/math/geometry/test/src/test_bounding_box.cpp index c6df82911d0..b3449f994e4 100644 --- a/common/math/geometry/test/test_bounding_box.cpp +++ b/common/math/geometry/test/src/test_bounding_box.cpp @@ -46,53 +46,64 @@ TEST(BoundingBox, getPointsFromBboxCustom) EXPECT_POINT_EQ(points[3], makePoint(6.5, -2.0, 1.0)); } -TEST(BoundingBox, get2DPolygonZeroPose) +/** + * @note Test obtaining polygon from bounding box with no transformation applied. + */ +TEST(BoundingBox, toPolygon2D_zeroPose) { - geometry_msgs::msg::Pose pose; - traffic_simulator_msgs::msg::BoundingBox bounding_box = makeBbox(2.0, 2.0, 2.0); - boost::geometry::model::polygon> poly = - math::geometry::toPolygon2D(pose, bounding_box); - EXPECT_EQ(poly.inners().size(), size_t(0)); - EXPECT_EQ(poly.outer().size(), size_t(5)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(1.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-1.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-1.0, -1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(1.0, -1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(1.0, 1.0)); + const auto pose = geometry_msgs::msg::Pose(); + const auto bounding_box = makeBbox(2.0, 2.0, 2.0); + const auto polygon = math::geometry::toPolygon2D(pose, bounding_box); + + ASSERT_EQ(polygon.inners().size(), static_cast(0)); + ASSERT_EQ(polygon.outer().size(), static_cast(5)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[0], makePoint(1.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[1], makePoint(-1.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[2], makePoint(-1.0, -1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[3], makePoint(1.0, -1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[4], makePoint(1.0, 1.0)); } -TEST(BoundingBox, get2DPolygonOnlyTranslation) +/** + * @note Test obtaining polygon from bounding box with only translation applied. + */ +TEST(BoundingBox, toPolygon2D_onlyTranslation) { - geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); - traffic_simulator_msgs::msg::BoundingBox bounding_box = makeBbox(2.0, 2.0, 2.0); - boost::geometry::model::polygon> poly = - math::geometry::toPolygon2D(pose, bounding_box); - EXPECT_EQ(poly.inners().size(), size_t(0)); - EXPECT_EQ(poly.outer().size(), size_t(5)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(2.0, 3.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(0.0, 3.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(0.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(2.0, 1.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(2.0, 3.0)); + const auto pose = makePose(1.0, 2.0); + const auto bounding_box = makeBbox(2.0, 2.0, 2.0); + const auto polygon = math::geometry::toPolygon2D(pose, bounding_box); + + ASSERT_EQ(polygon.inners().size(), static_cast(0)); + ASSERT_EQ(polygon.outer().size(), static_cast(5)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[0], makePoint(2.0, 3.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[1], makePoint(0.0, 3.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[2], makePoint(0.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[3], makePoint(2.0, 1.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[4], makePoint(2.0, 3.0)); } -TEST(BoundingBox, get2DPolygonFullPose) +/** + * @note Test obtaining polygon from bounding box with full transformation applied (translation + rotation). + */ +TEST(BoundingBox, toPolygon2D_fullPose) { - geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); - pose.orientation = math::geometry::convertEulerAngleToQuaternion( - geometry_msgs::build().x(0.0).y(0.0).z(30.0 * M_PI / 180.0)); - traffic_simulator_msgs::msg::BoundingBox bounding_box = makeBbox(2.0, 2.0, 2.0); - boost::geometry::model::polygon> poly = - math::geometry::toPolygon2D(pose, bounding_box); - EXPECT_EQ(poly.inners().size(), size_t(0)); - EXPECT_EQ(poly.outer().size(), size_t(5)); + const auto pose = makePose( + 1.0, 2.0, 0.0, + math::geometry::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z(30.0 * M_PI / 180.0))); + + const auto bounding_box = makeBbox(2.0, 2.0, 2.0); + const auto polygon = math::geometry::toPolygon2D(pose, bounding_box); + ASSERT_EQ(polygon.inners().size(), static_cast(0)); + ASSERT_EQ(polygon.outer().size(), static_cast(5)); + const double x = std::sqrt(2.0) * std::cos((30.0 + 45.0) * M_PI / 180.0); const double y = std::sqrt(2.0) * std::sin((30.0 + 45.0) * M_PI / 180.0); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[0], makePoint(x + 1.0, y + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[1], makePoint(-y + 1.0, x + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[2], makePoint(-x + 1.0, -y + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[3], makePoint(y + 1.0, -x + 2.0)); - EXPECT_BOOST_POINT_2D_AND_POINT_EQ(poly.outer()[4], makePoint(x + 1.0, y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[0], makePoint(x + 1.0, y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[1], makePoint(-y + 1.0, x + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[2], makePoint(-x + 1.0, -y + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[3], makePoint(y + 1.0, -x + 2.0)); + EXPECT_BOOST_POINT_2D_AND_POINT_EQ(polygon.outer()[4], makePoint(x + 1.0, y + 2.0)); } TEST(BoundingBox, getPolygonDistanceWithCollision) diff --git a/common/math/geometry/test/test_distance.cpp b/common/math/geometry/test/src/test_distance.cpp similarity index 100% rename from common/math/geometry/test/test_distance.cpp rename to common/math/geometry/test/src/test_distance.cpp diff --git a/common/math/geometry/test/test_transform.cpp b/common/math/geometry/test/src/test_transform.cpp similarity index 100% rename from common/math/geometry/test/test_transform.cpp rename to common/math/geometry/test/src/test_transform.cpp diff --git a/common/math/geometry/test/test_utils.hpp b/common/math/geometry/test/src/test_utils.hpp similarity index 100% rename from common/math/geometry/test/test_utils.hpp rename to common/math/geometry/test/src/test_utils.hpp diff --git a/common/math/geometry/test/vector3/CMakeLists.txt b/common/math/geometry/test/src/vector3/CMakeLists.txt similarity index 100% rename from common/math/geometry/test/vector3/CMakeLists.txt rename to common/math/geometry/test/src/vector3/CMakeLists.txt diff --git a/common/math/geometry/test/vector3/test_truncate_custom.cpp b/common/math/geometry/test/src/vector3/test_truncate_custom.cpp similarity index 100% rename from common/math/geometry/test/vector3/test_truncate_custom.cpp rename to common/math/geometry/test/src/vector3/test_truncate_custom.cpp diff --git a/common/math/geometry/test/vector3/test_truncate_msg.cpp b/common/math/geometry/test/src/vector3/test_truncate_msg.cpp similarity index 100% rename from common/math/geometry/test/vector3/test_truncate_msg.cpp rename to common/math/geometry/test/src/vector3/test_truncate_msg.cpp diff --git a/common/math/geometry/test/vector3/test_vector3.cpp b/common/math/geometry/test/src/vector3/test_vector3.cpp similarity index 93% rename from common/math/geometry/test/vector3/test_vector3.cpp rename to common/math/geometry/test/src/vector3/test_vector3.cpp index b07cfede363..c75f7177d3e 100644 --- a/common/math/geometry/test/vector3/test_vector3.cpp +++ b/common/math/geometry/test/src/vector3/test_vector3.cpp @@ -54,6 +54,9 @@ TEST(Vector3, hypot_customVector) EXPECT_DOUBLE_EQ(math::geometry::hypot(vec0, vec1), 6.0); } +/** + * @note Test function correctness with parameter that is ros message vector. + */ TEST(Vector3, norm_msgVector) { geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); @@ -75,6 +78,12 @@ TEST(Vector3, normalize_msgVector) const double norm = std::sqrt(14.0); EXPECT_VECTOR3_EQ( math::geometry::normalize(vec0), makeVector(1.0 / norm, 2.0 / norm, 3.0 / norm)); + + geometry_msgs::msg::Vector3 vec1; + EXPECT_DOUBLE_EQ(math::geometry::norm(vec1), 0.0); + + geometry_msgs::msg::Vector3 vec2 = makeVector(1.0, 0.0, 3.0); + EXPECT_DOUBLE_EQ(math::geometry::norm(vec2), std::sqrt(10.0)); } TEST(Vector3, normalize_customVector) @@ -184,25 +193,16 @@ TEST(Vector3, additionAssignment_customVector) EXPECT_VECTOR3_EQ(vec0, makeVector(0.0, 0.0, 2.0)); } -TEST(Vector3, vector3_getSizeZero) -{ - geometry_msgs::msg::Vector3 vec; - EXPECT_DOUBLE_EQ(math::geometry::norm(vec), 0.0); -} - -TEST(Vector3, vector3_getSize) -{ - geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); - EXPECT_DOUBLE_EQ(math::geometry::norm(vec), std::sqrt(10.0)); -} - -TEST(Vector3, vector3_normalizeZero) +/** + * @note Test function correctness when one of the vectors has length of 0. + */ +TEST(Vector3, normalize_zeroLength) { geometry_msgs::msg::Vector3 vec; EXPECT_THROW(math::geometry::normalize(vec), common::SimulationError); } -TEST(Vector3, vector3_normalize) +TEST(Vector3, normalize) { geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); vec = math::geometry::normalize(vec); diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp deleted file mode 100644 index 4c21cc35626..00000000000 --- a/common/math/geometry/test/test_line_segment.cpp +++ /dev/null @@ -1,642 +0,0 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include - -#include "expect_eq_macros.hpp" -#include "test_utils.hpp" - -TEST(LineSegment, initializeDifferentPoints) -{ - geometry_msgs::msg::Point point0 = makePoint(0.0, 0.0); - geometry_msgs::msg::Point point1 = makePoint(1.0, 1.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point0, point1)); - const math::geometry::LineSegment line_segment(point0, point1); - EXPECT_POINT_EQ(line_segment.start_point, point0); - EXPECT_POINT_EQ(line_segment.end_point, point1); -} - -TEST(LineSegment, initializeIdenticalPoints) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, point)); - const math::geometry::LineSegment line_segment(point, point); - EXPECT_POINT_EQ(line_segment.start_point, point); - EXPECT_POINT_EQ(line_segment.end_point, point); -} - -TEST(LineSegment, initializeVector) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 1.0)); - const math::geometry::LineSegment line_segment(point, vec, 1.0); - EXPECT_POINT_EQ(line_segment.start_point, point); - EXPECT_POINT_EQ(line_segment.end_point, makePoint(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0)); -} - -TEST(LineSegment, initializeVectorZero) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - geometry_msgs::msg::Vector3 vec = makeVector(0.0, 0.0); - EXPECT_THROW( - const math::geometry::LineSegment line_segment(point, vec, 1.0), common::SimulationError); -} - -TEST(LineSegment, initializeVectorZeroLength) -{ - geometry_msgs::msg::Point point = makePoint(0.0, 0.0); - geometry_msgs::msg::Vector3 vec = makeVector(1.0, 1.0); - EXPECT_NO_THROW(const math::geometry::LineSegment line_segment(point, vec, 0.0)); - const math::geometry::LineSegment line_segment(point, vec, 0.0); - EXPECT_POINT_EQ(line_segment.start_point, point); - EXPECT_POINT_EQ(line_segment.end_point, point); -} - -TEST(LineSegment, isIntersect2DDisjoint) -{ - const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); - EXPECT_FALSE(line0.isIntersect2D(line1)); -} - -TEST(LineSegment, isIntersect2DIntersect) -{ - const math::geometry::LineSegment line0(makePoint(1.0, 1.0), makePoint(2.0, 1.0)); - const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makeVector(1.0, 1.0), 3.0); - EXPECT_TRUE(line0.isIntersect2D(line1)); -} - -TEST(LineSegment, isIntersect2DIdentical) -{ - const math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - EXPECT_TRUE(line.isIntersect2D(line)); -} - -TEST(LineSegment, getIntersection2DDisjoint) -{ - const math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); - const math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); - EXPECT_FALSE(line0.getIntersection2D(line1)); -} - -TEST(LineSegment, getVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_VECTOR3_EQ(line.getVector(), makeVector(1.0, 1.0, 1.0)); -} - -TEST(LineSegment, getVectorZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_VECTOR3_EQ(line.getVector(), makeVector(0.0, 0.0, 0.0)); -} - -TEST(LineSegment, getNormalVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(-1.0, 1.0, 0.0)); -} - -TEST(LineSegment, getNormalVector_zeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_VECTOR3_EQ(line.getNormalVector(), makeVector(0.0, 0.0, 0.0)); -} - -TEST(LineSegment, get2DVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_VECTOR3_EQ(line.get2DVector(), makeVector(1.0, 1.0, 0.0)); -} - -TEST(LineSegment, get2DVectorZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_VECTOR3_EQ(line.get2DVector(), makeVector(0.0, 0.0, 0.0)); -} - -TEST(LineSegment, getLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_DOUBLE_EQ(line.getLength(), std::sqrt(3.0)); -} - -TEST(LineSegment, getLengthZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_DOUBLE_EQ(line.getLength(), 0.0); -} - -TEST(LineSegment, get2DLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0)); - EXPECT_DOUBLE_EQ(line.get2DLength(), std::sqrt(2.0)); -} - -TEST(LineSegment, get2DLengthZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_DOUBLE_EQ(line.get2DLength(), 0.0); -} - -TEST(LineSegment, getSlope) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 3.0, 4.0)); - EXPECT_DOUBLE_EQ(line.getSlope(), 0.5); -} - -TEST(LineSegment, getSlopeZeroLength) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(1.0, 2.0, 3.0)); - EXPECT_TRUE(std::isnan(line.getSlope())); -} - -TEST(LineSegment, getSquaredDistanceIn2D) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_DOUBLE_EQ(line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), 0.5, false), 8.0); -} - -TEST(LineSegment, getSquaredDistanceIn2D_denormalize) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_DOUBLE_EQ( - line.getSquaredDistanceIn2D(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), 8.0); -} - -TEST(LineSegment, getSquaredDistanceVector) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_VECTOR3_EQ( - line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), 0.5, false), - makeVector(-2.0, -2.0, -2.0)); -} - -TEST(LineSegment, getSquaredDistanceVector_denormalize) -{ - const math::geometry::LineSegment line(makePoint(1.0, 2.0, 3.0), makePoint(3.0, 4.0, 5.0)); - EXPECT_VECTOR3_EQ( - line.getSquaredDistanceVector(makePoint(0.0, 1.0, 2.0), std::sqrt(3.0), true), - makeVector(-2.0, -2.0, -2.0)); -} - -TEST(LineSegment, getLineSegments) -{ - const std::vector points{ - makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), - makePoint(4.0, 5.0, 6.0)}; - const std::vector lines = - math::geometry::getLineSegments(points, false); - EXPECT_EQ(lines.size(), size_t(3)); - EXPECT_POINT_EQ(lines[0].start_point, points[0]); - EXPECT_POINT_EQ(lines[0].end_point, points[1]); - EXPECT_POINT_EQ(lines[1].start_point, points[1]); - EXPECT_POINT_EQ(lines[1].end_point, points[2]); - EXPECT_POINT_EQ(lines[2].start_point, points[2]); - EXPECT_POINT_EQ(lines[2].end_point, points[3]); -} - -TEST(LineSegment, getLineSegments_closeStartEnd) -{ - const std::vector points{ - makePoint(1.0, 2.0, 3.0), makePoint(2.0, 3.0, 4.0), makePoint(3.0, 4.0, 5.0), - makePoint(4.0, 5.0, 6.0)}; - const std::vector lines = - math::geometry::getLineSegments(points, true); - EXPECT_EQ(lines.size(), size_t(4)); - EXPECT_POINT_EQ(lines[0].start_point, points[0]); - EXPECT_POINT_EQ(lines[0].end_point, points[1]); - EXPECT_POINT_EQ(lines[1].start_point, points[1]); - EXPECT_POINT_EQ(lines[1].end_point, points[2]); - EXPECT_POINT_EQ(lines[2].start_point, points[2]); - EXPECT_POINT_EQ(lines[2].end_point, points[3]); - EXPECT_POINT_EQ(lines[3].start_point, points[3]); - EXPECT_POINT_EQ(lines[3].end_point, points[0]); -} - -TEST(LineSegment, getLineSegmentsVectorEmpty) -{ - const std::vector points; - const std::vector lines = math::geometry::getLineSegments(points); - EXPECT_EQ(lines.size(), size_t(0)); -} - -TEST(LineSegment, getLineSegmentsVectorIdentical) -{ - geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); - const std::vector points{point, point, point, point}; - const std::vector lines = - math::geometry::getLineSegments(points, false); - EXPECT_EQ(lines.size(), size_t(3)); - EXPECT_POINT_EQ(lines[0].start_point, point); - EXPECT_POINT_EQ(lines[0].end_point, point); - EXPECT_POINT_EQ(lines[1].start_point, point); - EXPECT_POINT_EQ(lines[1].end_point, point); - EXPECT_POINT_EQ(lines[2].start_point, point); - EXPECT_POINT_EQ(lines[2].end_point, point); -} - -TEST(LineSegment, getLineSegmentsVectorIdentical_closeStartEnd) -{ - geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); - const std::vector points{point, point, point, point}; - const std::vector lines = - math::geometry::getLineSegments(points, true); - EXPECT_EQ(lines.size(), size_t(4)); - EXPECT_POINT_EQ(lines[0].start_point, point); - EXPECT_POINT_EQ(lines[0].end_point, point); - EXPECT_POINT_EQ(lines[1].start_point, point); - EXPECT_POINT_EQ(lines[1].end_point, point); - EXPECT_POINT_EQ(lines[2].start_point, point); - EXPECT_POINT_EQ(lines[2].end_point, point); - EXPECT_POINT_EQ(lines[3].start_point, point); - EXPECT_POINT_EQ(lines[3].end_point, point); -} - -TEST(LineSegment, getSValue) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, false); - EXPECT_TRUE(s); - if (s) { - EXPECT_DOUBLE_EQ(s.value(), 2.0 / 3.0); - } -} - -TEST(LineSegment, getSValue_denormalize) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(2.0, 2.0, 2.0), 1.0, true); - EXPECT_TRUE(s); - if (s) { - EXPECT_DOUBLE_EQ(s.value(), std::hypot(2.0, 2.0, 2.0)); - } -} - -TEST(LineSegment, getSValue_outOfRange) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, false); - EXPECT_FALSE(s); -} - -TEST(LineSegment, getSValue_outOfRangeDenormalize) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 3.0)); - const auto s = line.getSValue(makePose(4.0, 4.0, 4.0), 1.0, true); - EXPECT_FALSE(s); -} - -TEST(LineSegment, getSValue_parallel) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); - const auto s = line.getSValue( - makePose( - 1.0, 0.0, 0.0, - math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), - 1000.0, false); - EXPECT_FALSE(s); -} - -TEST(LineSegment, getSValue_parallelDenormalize) -{ - math::geometry::LineSegment line(makePoint(0.0, 0.0, 0.0), makePoint(3.0, 3.0, 0.0)); - const auto s = line.getSValue( - makePose( - 1.0, 0.0, 0.0, - math::geometry::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, M_PI_4 * 3.0))), - 1000.0, true); - EXPECT_FALSE(s); -} - -/// @brief In this test case, testing the `LineSegment::getPoint` function can find the point on the line segment with start point (x,y,z) = (0,0,0) and end point (x,y,z) = (1,1,1) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, GetPoint) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(0).z(0), - geometry_msgs::build().x(1).y(1).z(1)); - /// @note If s = 0 and denormalize_s = false, the return value of the `getPoint` function should be a start point of the `line`. - /// If the `denormalize_s = false`, the return value `s` of the function is normalized and in range `s = [0,1]`. - // [Snippet_getPoint_with_s_0] - EXPECT_POINT_EQ( - line.getPoint(0, false), geometry_msgs::build().x(0).y(0).z(0)); - // [Snippet_getPoint_with_s_0] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_with_s_0 - - /// @note If s = 0 and denormalize_s = false, the return value of the `LineSegment::getPoint` function should be a end point of the `line`. - /// If the `denormalize_s = false`, the return value `s` of the function is normalized and in range `s = [0,1]`. - // [Snippet_getPoint_with_s_1] - EXPECT_POINT_EQ( - line.getPoint(1, false), geometry_msgs::build().x(1).y(1).z(1)); - // [Snippet_getPoint_with_s_1] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_with_s_1 - - /// @note If s = sqrt(3) and denormalize_s = true, the return value of the `LineSegment::getPoint` function should be a end point of the `line`. - /// If the `denormalize_s = true`, the return value `s` is denormalized and it returns the value `s` times the length of the curve. - // [Snippet_getPoint_with_sqrt_3_denormalized] - EXPECT_POINT_EQ( - line.getPoint(std::sqrt(3), true), - geometry_msgs::build().x(1).y(1).z(1)); - // [Snippet_getPoint_with_sqrt_3_denormalized] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_with_sqrt_3_denormalized - - /// @note If s is not in range s = [0,1], testing the `LineSegment::getPoint` function can throw error. If s is not in range s = [0,1], it means the point is not on the line. - // [Snippet_getPoint_out_of_range] - EXPECT_THROW(line.getPoint(2, false), common::SimulationError); - EXPECT_THROW(line.getPoint(-1, false), common::SimulationError); - // [Snippet_getPoint_out_of_range] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_out_of_range - - /// @note If s is not in range s = [0,sqrt(3)] and denormalize, testing the `LineSegment::getPoint` function can throw error. If s is not in range s = [0,sqrt(3)], it means the point is not on the line. - // [Snippet_getPoint_out_of_range_with_denormalize] - EXPECT_THROW(line.getPoint(2, true), common::SimulationError); - EXPECT_THROW(line.getPoint(-1, true), common::SimulationError); - EXPECT_NO_THROW(line.getPoint(0, true)); - EXPECT_NO_THROW(line.getPoint(std::sqrt(3.0), true)); - // [Snippet_getPoint_out_of_range_with_denormalize] - /// @snippet test/test_line_segment.cpp Snippet_getPoint_out_of_range_with_denormalize - } -} - -/// @brief In this test case, testing the `LineSegment::getPose` function can find the pose on the line segment with start point (x,y,z) = (0,0,0) and end point (x,y,z) = (0,0,1) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, GetPose) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(0).z(0), - geometry_msgs::build().x(0).y(0).z(1)); - /// @note When the `s = 0`(1st argument of the `LineSegment::getPose` function), the return value of the `LineSegment::getPose` function should be a start point of the `line`. - /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis. - // [Snippet_getPose_with_s_0] - EXPECT_POSE_EQ( - line.getPose(0, false, false), - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(0)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))); - EXPECT_POSE_EQ( - line.getPose(0, false, true), - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(0)) - .orientation(math::geometry::convertEulerAngleToQuaternion( - geometry_msgs::build().x(0).y(-M_PI * 0.5).z(0)))); - // [Snippet_getPose_with_s_0] - /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_0 - - /// @note When the `s = 1`(1st argument of the `LineSegment::getPose` function), the return value of the `LineSegment::getPose` function should be an end point of the `line`. - /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis. - // [Snippet_getPose_with_s_1] - EXPECT_POSE_EQ( - line.getPose(1, false, false), - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(1)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))); - EXPECT_POSE_EQ( - line.getPose(1, false, true), - geometry_msgs::build() - .position(geometry_msgs::build().x(0).y(0).z(1)) - .orientation(math::geometry::convertEulerAngleToQuaternion( - geometry_msgs::build().x(0).y(-M_PI * 0.5).z(0)))); - // [Snippet_getPose_with_s_1] - /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_1 - } -} - -/// @brief In this test case, we check collision with the point and the line segment with start point (x,y,z) = (0,-1,0) and end point (x,y,z) = (0,1,0) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, isIntersect2D) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(-1).z(0), - geometry_msgs::build().x(0).y(1).z(0)); - /// @note Testing the `LineSegment::isIntersect2D` function can find intersection with point (0,0,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_0_0] - EXPECT_TRUE( - line.isIntersect2D(geometry_msgs::build().x(0).y(0).z(0))); - // [Snippet_isIntersect2D_with_point_0_0_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_0_0 - - /// @note Testing the `LineSegment::isIntersect2D` function can find intersection with point (0,0,-1) - // [Snippet_isIntersect2D_with_point_0_0_-1] - EXPECT_TRUE( - line.isIntersect2D(geometry_msgs::build().x(0).y(0).z(-1))); - // [Snippet_isIntersect2D_with_point_0_0_-1] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_0_-1 - - /// @note Testing the `LineSegment::isIntersect2D` function can find intersection with point (0,1,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_1_0] - EXPECT_TRUE( - line.isIntersect2D(geometry_msgs::build().x(0).y(1).z(0))); - // [Snippet_isIntersect2D_with_point_0_1_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_1_0 - - /// @note Testing the `LineSegment::isIntersect2D` function can find that there is no intersection with point (0,2,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_2_0] - EXPECT_FALSE( - line.isIntersect2D(geometry_msgs::build().x(0).y(2).z(0))); - // [Snippet_isIntersect2D_with_point_0_2_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_2_0 - - /// @note Testing the `LineSegment::isIntersect2D` function can find that there is no intersection with point (0,-2,0) and `line`. - // [Snippet_isIntersect2D_with_point_0_-2_0] - EXPECT_FALSE( - line.isIntersect2D(geometry_msgs::build().x(0).y(-2).z(0))); - // [Snippet_isIntersect2D_with_point_0_-2_0] - /// @snippet test/test_line_segment.cpp Snippet_isIntersect2D_with_point_0_-2_0 - } -} - -/// @brief In this test case, we check collision with the line segment with start point (x,y,z) = (0,-1,0) and end point (x,y,z) = (0,1,0) in the cartesian coordinate system. (variable name `line`). -TEST(LineSegment, getIntersection2DSValue) -{ - { - math::geometry::LineSegment line( - geometry_msgs::build().x(0).y(-1).z(0), - geometry_msgs::build().x(0).y(1).z(0)); - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,0,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 0.5. - * If so, the variable `collision_s` should be `std::optional(0.5)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_0_0] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(0).z(0), false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.5); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_0_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_0_0 - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,1,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 1.0. - * If so, the variable `collision_s` should be `std::optional(1.0)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_1_0] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(1).z(0), false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 1.0); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_1_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_1_0 - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,1,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 1.0. - * And, the 2nd argument of the `LineSegment::getIntersection2DSValue` (denormalized_s) is true, so the return value should be 1.0 (normalized s value.) * 2.0 (length of the `line`) = 2.0. - * If so, the variable `collision_s` should be `std::optional(2.0)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_1_0_denormalized] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(1).z(0), true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 2.0); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_1_0_denormalized] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_1_0_denormalized - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find a collision with the point with (x,y,z) = (0,0,0) in the cartesian coordinate system. - * In the frenet coordinate system along the `line`, the s value should be 0.5. - * And, the 2nd argument of the `LineSegment::getIntersection2DSValue` (denormalized_s) is true, so the return value should be 0.5 (normalized s value.) * 2.0 (length of the `line`) = 1.0. - * If so, the variable `collision_s` should be `std::optional(1.0)`. - */ - // [Snippet_getIntersection2DSValue_with_point_0_0_0_denormalized] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(0).y(0).z(0), true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 1.0); - } - } - // [Snippet_getIntersection2DSValue_with_point_0_0_0_denormalized] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_0_0_0_denormalized - - /** - * @note Testing the `LineSegment::getIntersection2DSValue` function can find that the point with (x,y,z) = (1,0,0) in the cartesian coordinate system does not collide to `line.`. - * If the function works valid, the variable `collision_s` should be `std::nullopt`. - */ - // [Snippet_getIntersection2DSValue_with_point_1_0_0] - { - const auto collision_s = line.getIntersection2DSValue( - geometry_msgs::build().x(1).y(0).z(0), false); - EXPECT_FALSE(collision_s); - } - // [Snippet_getIntersection2DSValue_with_point_1_0_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2DSValue_with_point_1_0_0 - - { // parallel no denormalize - EXPECT_THROW( - line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(0.0, -1.0), makePoint(0.0, 1.0)), false), - common::SimulationError); - } - { // parallel denormalize - EXPECT_THROW( - line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(0.0, -1.0), makePoint(0.0, 1.0)), true), - common::SimulationError); - } - { // intersect no denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 0.5), makePoint(1.0, 0.5)), false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.75); - } - } - { // intersect denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 0.5), makePoint(1.0, 0.5)), true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 1.5); - } - } - { // no intersect no denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 1.5), makePoint(1.0, 1.5)), false); - EXPECT_FALSE(collision_s); - } - { // no intersect denormalize - const auto collision_s = line.getIntersection2DSValue( - math::geometry::LineSegment(makePoint(-1.0, 1.5), makePoint(1.0, 1.5)), true); - EXPECT_FALSE(collision_s); - } - - /** - * @note Testing the `LineSegment::getIntersection2D` function can throw error getting intersection with exact same line segment. - * In this case, any s value can be a intersection point, so we cannot return single value. - */ - // [Snippet_getIntersection2D_with_line] - { - EXPECT_THROW(line.getIntersection2D(line), common::SimulationError); - } - // [Snippet_getIntersection2D_with_line] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2D_with_line - - /** - * @note Testing the `LineSegment::getIntersection2D` function can throw error getting intersection with part of the line segment `line`. - * In this case, any s value can be a intersection point, so we cannot return single value. - */ - // [Snippet_getIntersection2D_with_part_of_line] - { - EXPECT_THROW( - line.getIntersection2D(math::geometry::LineSegment( - geometry_msgs::build().x(0).y(-1).z(0), - geometry_msgs::build().x(0).y(0).z(0))), - common::SimulationError); - } - // [Snippet_getIntersection2D_with_part_of_line] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2D_with_part_of_line - - /// @note @note Testing the `LineSegment::getIntersection2D` function can find collision with the line segment with start point (x,y,z) = (1,0,0) and end point (x,y,z) = (-1,0,0) in the cartesian coordinate system and `line`. - // [Snippet_getIntersection2D_line_1_0_0_-1_0_0] - { - const auto point = line.getIntersection2D(math::geometry::LineSegment( - geometry_msgs::build().x(1).y(0).z(0), - geometry_msgs::build().x(-1).y(0).z(0))); - EXPECT_TRUE(point); - if (point) { - EXPECT_POINT_EQ( - point.value(), geometry_msgs::build().x(0).y(0).z(0)); - } - } - // [Snippet_getIntersection2D_line_1_0_0_-1_0_0] - /// @snippet test/test_line_segment.cpp Snippet_getIntersection2D_line_1_0_0_-1_0_0 - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index e1ddf9f9b3b..a9d62d915ea 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 5239189d015..64e51c98484 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 3.3.0 + 4.3.7 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index 0258b39f883..7a0944ff58f 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index fcf3f77b8af..9755ac5efb5 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 3.3.0 + 4.3.7 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index b6124b6bb2a..8499636f9da 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 87e6dbcc8de..b9f3156b62f 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 3.3.0 + 4.3.7 none Tatsuya Yamasaki Apache License 2.0 diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 1157fd965c9..b4bc8293b5f 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -36,6 +36,7 @@ | `/perception/object_recognition/ground_truth/objects` | [`autoware_auto_perception_msgs/msg/TrackedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrackedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | | `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | +| `/sensing/imu/imu_data` | [`sensor_msgs/msg/Imu`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/Imu.msg) | | | `/perception/traffic_light_recognition/traffic_signals` | [`autoware_auto_perception_msgs/msg/TrafficSignalArray`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl) | for `architecture_type` equal to `awf/universe` | | `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | for `architecture_type` greater or equal to `awf/universe/20230906` | | `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | diff --git a/docs/developer_guide/ConfiguringPerceptionTopics.md b/docs/developer_guide/ConfiguringPerceptionTopics.md index 22495fd1f5b..8708cd8e5fe 100644 --- a/docs/developer_guide/ConfiguringPerceptionTopics.md +++ b/docs/developer_guide/ConfiguringPerceptionTopics.md @@ -24,13 +24,16 @@ where `` and `` can be set to: | Name | Value | Default | Description | |--------------------------------------------|-----------------------------------------------|:-------:|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| `detectedObjectMissingProbability` | A `double` type value between `0.0` and `1.0` | `0.0` | Do not publish the perception topic with the given probability. | -| `detectedObjectPositionStandardDeviation` | A positive `double` type value | `0.0` | Randomize the positions of other vehicles included in the perception topic according to the given standard deviation. | -| `detectedObjectPublishingDelay` | A positive `double` type value | `0.0` | Delays the publication of the perception topic by the specified number of seconds. | -| `detectedObjectGroundTruthPublishingDelay` | A positive `double` type value | `0.0` | Delays the publication of the perception ground truth topic by the specified number of seconds. | +| `detectedObjectMissingProbability` | A `double` type value between `0.0` and `1.0` | `0.0` | Do not publish the perception topic with the given probability. | +| `detectedObjectPositionStandardDeviation` | A positive `double` type value | `0.0` | Randomize the positions of other vehicles included in the perception topic according to the given standard deviation. | +| `detectedObjectPublishingDelay` | A positive `double` type value | `0.0` | Delays the publication of the perception topic by the specified number of seconds. | +| `detectedObjectGroundTruthPublishingDelay` | A positive `double` type value | `0.0` | Delays the publication of the perception ground truth topic by the specified number of seconds. | | `detectionSensorRange` | A positive `double` type value | `300.0` | Specifies the sensor detection range for detected object. | | `isClairvoyant` | A `boolean` type value | `false` | Specifies whether the detected object is a Clairvoyant. If this parameter is not defined explicitly, the property of `detectionSensorRange` is not reflected and only detected object detected by lidar is published. | -| `randomSeed` | A positive `integer` type value | `0` | Specifies the seed value for the random number generator. | +| `pointcloudChannels` | A positive `integer` type value | `16` | Number of channels of pseudo LiDAR inside the simulator used to generate pointclouds. | +| `pointcloudHorizontalResolution` | A positive `double` type value | `1.0` | Horizontal angular resolution of the pseudo LiDAR inside the simulator used to generate the pointcloud. | +| `pointcloudVerticalFieldOfView` | A positive `double` type value | `30.0` | Vertical field of view of the pseudo LiDAR inside the simulator used to generate the pointcloud. | +| `randomSeed` | A positive `integer` type value | `0` | Specifies the seed value for the random number generator. | These properties are not exclusive. In other words, multiple properties can be specified at the same time. However, these properties only take effect for @@ -161,7 +164,7 @@ this problem by setting an interval of the specified number of seconds between `scenario_simulator_v2` generating a perception result and publishing it. **Specification** - The property's value must be a positive real number. The -unit is seconds. It is an error if the value is negative.　Since the delay is +unit is seconds. It is an error if the value is negative. Since the delay is set to the same value for each topic, it is not possible to delay only a specific topic. @@ -230,6 +233,114 @@ it reaches the receiver node. value: "3" ``` +## Property `pointcloudChannels` + +**Summary** - Number of channels of pseudo LiDAR inside the simulator used to +generate pointclouds. + +**Purpose** - The `simple_sensor_simulator` simulates a simple LiDAR, such as a +horizontally rotating vertically aligned laser, typical of the VLP-16, to +generate a pointcloud. The default settings produce a pointcloud that exactly +mimics the sensing results from the VLP-16. However, VLP-16 is a relatively low +pointcloud density LiDAR used with Autoware, so if a higher pointcloud density +LiDAR is to be installed, it will be necessary to simulate a scenario with a +higher pointcloud density to match the actual vehicle. This property addresses +this issue by providing a means to specify the number of pseudo LiDAR channels. + +**Specification** - The property value must be a real number greater than or +equal to 1. Zero or negative values are errors. The upper limit of values is +the maximum value of a 64-bit unsigned integer, but computer performance +effectively limits the value to much lower values. + +**Guarantee** - The `simple_sensor_simulator` does not simulate a realistic +LiDAR. For example, in the case of a LiDAR with a mechanically rotating laser +structure, the resulting point cloud will be distorted when moving at high +speeds, but `simple_sensor_simulator` cannot simulate such behavior and +produces an undistorted pointcloud. + +**Default behavior** - If the property is not specified, the default value is +`"16"`. When the properties `pointcloudChannels` and +`pointcloudVerticalFieldOfView` are both at their default values, the behavior +mimics Velodyne VLP-16. + +**[Example](https://github.com/tier4/scenario_simulator_v2/blob/487556437b448186e2de484f5130eb2b1d015e74/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml)** - +``` + ObjectController: + Controller: + name: '...' + Properties: + Property: + - name: 'isEgo' + value: 'true' + - name: 'pointcloudChannels' + value: '128' +``` + +## Property `pointcloudHorizontalResolution` + +**Summary** - Horizontal angular resolution of the pseudo LiDAR inside the +simulator used to generate the pointcloud. + +**Purpose** - To address the same issues as the property `pointcloudChannels`, +this property provides a means to specify the angular resolution of the pseudo +LiDAR. + +**Specification** - The property value must be a real number greater than zero. +The unit is degrees. If the value is zero or negative, it is an error. + +**Guarantee** - Same as one for `pointcloudChannels` + +**Default behavior** - If the property is not specified, the default value is +`"1.0"`. + +**[Example](https://github.com/tier4/scenario_simulator_v2/blob/487556437b448186e2de484f5130eb2b1d015e74/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml)** - +``` + ObjectController: + Controller: + name: '...' + Properties: + Property: + - name: 'isEgo' + value: 'true' + - name: 'pointcloudHorizontalResolution' + value: '1.5' +``` + +## Property `pointcloudVerticalFieldOfView` + +**Summary** - Vertical field of view of the pseudo LiDAR inside the simulator +used to generate the pointcloud. + +**Purpose** - To address the same issues as the property `pointcloudChannels`, +this property provides a means to specify the vertical field of view of the +pseudo LiDAR. + +**Specification** - The property value must be a real number greater than zero. +The unit is degrees. A value of zero or negative is an error. The specified +angle is assigned equally up and down to the horizontal plane as viewed from +the pseudo LiDAR. For example, if the value `30.0` is specified, the vertical +field of view is +15° to -15°. + +**Guarantee** - Same as one for `pointcloudChannels` + +**Default behavior** - If the property is not specified, the default value is +`"30.0"`. When the properties `pointcloudChannels` and +`pointcloudVerticalFieldOfView` are both at their default values, the behavior +mimics Velodyne VLP-16. + +**[Example](https://github.com/tier4/scenario_simulator_v2/blob/487556437b448186e2de484f5130eb2b1d015e74/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml)** - +``` + ObjectController: + Controller: + name: '...' + Properties: + Property: + - name: 'isEgo' + value: 'true' + - name: 'pointcloudVerticalFieldOfView' + value: '45.678' +``` + ## Property `randomSeed` **Summary** - Specifies the seed value for the random number generator. diff --git a/docs/developer_guide/Longitudinal_control.md b/docs/developer_guide/Longitudinal_control.md new file mode 100644 index 00000000000..65cf253001e --- /dev/null +++ b/docs/developer_guide/Longitudinal_control.md @@ -0,0 +1,203 @@ +# Longitudinal Control + +traffic_simulator has various ways to control the longitudinal behavior of the entity. + +## Basic Information +### target_speed +In this document, you will see variant called `target_speed` which is the target speed of the entity. The entity will always try to reach the target speed under the constraints such as acceleration or deceleration set by the user or if not by the default value. If `target_speed` is `NULL`, the entity will keep or change the speed with some other constraints set by the user or if not by the default value. + +### Set functions +Set functions directly changes the speed, acceleration, or deceleration of the entity. This will be executed immediately, even while the entity is trying to reach the target speed. It will ignore the physics of the entity and set the value directly. + +## List of Longitudinal Controllers + +| Name | Description | +| --------------------------------------------------------- | ----------------------------------------------- | +| [**requestSpeedChange**](#requestSpeedChange) | Changes the speed of the entity. | +| [**requestSynchronize**](#requestSynchronize) | Synchronizes the entity with the target entity. | +| [**setLinearVelocity**](#setLinearVelocity) | Sets the linear velocity of the entity. | +| [**setTwist**](#setTwist) | Sets the twist of the entity. | +| [**setAcceleration**](#setAcceleration) | Sets the acceleration of the entity. | +| [**setAccelerationLimit**](#setAccelerationLimit) | Sets the acceleration limit of the entity. | +| [**setAccelerationRateLimit**](#setAccelerationRateLimit) | Sets the acceleration rate limit of the entity. | +| [**setDecelerationLimit**](#setDecelerationLimit) | Sets the deceleration limit of the entity. | +| [**setDecelerationRateLimit**](#setDecelerationRateLimit) | Sets the deceleration rate limit of the entity. | +| [**setVelocityLimit**](#setVelocityLimit) | Sets the velocity limit of the entity. | + +## Details +### requestSpeedChange +By using `API::requestSpeedChange`, you can change the speed of the entity. +MiscObjectEntity can not be controlled by this API. +For EgoEntity, speed can only be changed before the simulation starts. +API takes several types of value sets. Description of each type is as follows. + +#### Values +| Value | Type | Description | +| ------------ | ------ | -------------------------------------------------------------- | +| name | string | Name of the entity. | +| target_speed | double | Target speed of the entity. | +| continuous | bool | If true the entity will keep the speed until the next command. | + +The function will change entities `target_speed` to given immediately. +If `continuous` is set to `false`, job to accelerate to target speed will be deleted after the velocity has reached the target speed. If set to `true`, the entity will keep the speed until the next longitudinal control command ordered. It will accelerate on maximum acceleration rate set previously. + +#### Values +| Value | Type | Description | +| ------------ | ------------------------ | -------------------------------------------------------------- | +| name | string | Name of the entity. | +| target_speed | double | Target speed of the entity. | +| transition | speed_change::Transition | Transition type. | +| constraint | speed_change::Constraint | Constraint type. | +| continuous | bool | If true the entity will keep the speed until the next command. | + +If `continuous` is set to `false`, job to accelerate to target speed will be deleted after the velocity has reached the target speed. If set to `true`, the entity will keep the speed until the next longitudinal control command ordered. + +##### Longitudinal Acceleration +When `constraint.type` is set to `LONGITUDINAL_ACCELERATION`, the entity will accelerate to the target speed with acceleration of `constraint.value`. +- If `transition` is set to `LINEAR`, the entity will accelerate to the target speed linearly. +- If `transition` is set to `AUTO`, it will change the maximum acceleration speed of the entity and append the job to change the target speed of the entity to the job queue. After the entity reaches the target speed, it will change the maximum acceleration speed back to the original value. +- If `transition` is set to `STEP`, it will change the speed of the entity to the target speed immediately. It will ignore the acceleration rate set by the user. + +##### Time Constraint Acceleration +When `constraint.type` is set to `TIME`, the entity will accelerate to the target speed by the time of `constraint.value`. +For `transition` it is the same as `LONGITUDINAL_ACCELERATION`. + +##### None Constraint Acceleration +When `constraint` is set to `NONE`, is will append the job to change the target_speed of the entity to the job queue. + +```mermaid +flowchart LR + A[requestSpeedChange] --> B{constraint.type}; + B -- LONGITUDINAL_ACCELERATION --> C{transition}; + C -- LINEAR --> D[SetLinearAcceleration]; + D -- AUTO --> C; + C -- AUTO --> E[SetAccelerationRateLimit + or + SetDecelerationRateLimit]; + E --> F[[ChangeTargetSpeed]]; + C -- STEP --> G[[changeTargetSpeed]]; + G --> H[setLinearVelocity]; + + B -- TIME --> I{transition}; + I -- LINEAR --> J[SetLinearAcceleration]; + J -- AUTO --> I; + I -- AUTO --> K[SetAccelerationRateLimit + or + SetDecelerationRateLimit]; + K --> L[[ChangeTargetSpeed]]; + I -- STEP --> M[[changeTargetSpeed]]; + M --> N[setLinearVelocity]; + + B -- NONE ---> O[[ChangeTargetSpeed]]; +``` + +#### Values +By using `speed_change::RelativeTargetSpeed` you can set the target speed of the entity relative to another entity. +| Value | Type | Description | +| ------------ | --------------------------------- | -------------------------------------------------------------- | +| name | string | Name of the entity. | +| target_speed | speed_change::RelativeTargetSpeed | Relative target speed. | +| continuous | bool | If true the entity will keep the speed until the next command. | + +If `continuous` is set to `false`, job to accelerate to target speed will be deleted after the velocity has reached the target speed. If set to `true`, the entity will keep the speed until the next longitudinal control command ordered. + +`target_speed.reference_entity_name` is the name of the entity that the target speed is relative to. The target speed of the entity will be same as the target speed of the reference entity. +- By setting `target_speed.type` to `DELTA`, you can set the target speed of the entity to be the target speed of the reference entity plus the value of `target_speed.value`. +- By setting `target_speed.type` to `FACTOR`, you can set the target speed of the entity to be the target speed of the reference entity multiplied by the value of `target_speed.value`. + +#### Values +By using `speed_change::RelativeTargetSpeed` you can set the target speed of the entity relative to another entity. +| Value | Type | Description | +| ------------ | --------------------------------- | -------------------------------------------------------------- | +| name | string | Name of the entity. | +| target_speed | speed_change::RelativeTargetSpeed | Relative target speed. | +| transition | speed_change::Transition | Transition type. | +| constraint | speed_change::Constraint | Constraint type. | +| continuous | bool | If true the entity will keep the speed until the next command. | +It works the same as explained before. + +### requestSynchronize +By using `API::requestSynchronize`, you can request the entity to adjust speed to stop at the designated lanelet by the time target entity crosses the another designated lanelet. + +| Value | Description | +| ---------------- | ---------------------------------------------------------------------- | +| name | Name of the entity. | +| target_name | Name of the target entity. | +| target_sync_pose | Target lanelet pose for target entity. | +| entity_target | Target lanelet pose for controlling entity. | +| target_speed | Target speed for controlling entity (meter per second). | +| tolerance | Tolerance for how much margin to accept to stop at the target (meter). | + +![requestSynchronize](images/Longitudinal_control/synchronizedAction.png) +As shown in the image, the entity will adjust it's speed to `target_speed` on `entity_target` at the time the target entity crosses the `target_sync_pose`. `tolerance` is the margin of error for the entity to stop at the `entity_target`. The target entity could be set by giving the name of the entity as `target_name`. + +### setLinearVelocity +By using `API::setLinearVelocity`, you can set the linear velocity of the entity immediately. It will ignore the physics of the entity and set the value directly. + +| Value | Type | Description | +| --------------- | ------ | ------------------------------ | +| name | string | Name of the entity. | +| linear_velocity | double | Linear velocity of the entity. | + + +### setTwist +By using `API::setTwist`, you can set the twist of the entity immediately. It will ignore the physics of the entity and set the value directly. + +| Value | Type | Description | +| ----- | ------------------------- | -------------------- | +| name | string | Name of the entity. | +| twist | geometry_msgs::msg::Twist | Twist of the entity. | + +### setAcceleration + +By using `API::setAcceleration`, you can set the acceleration of the entity immediately. It will ignore the physics of the entity and set the value directly. + +| Value | Type | Description | +| ------------ | ------------------------- | --------------------------- | +| name | string | Name of the entity. | +| acceleration | geometry_msgs::msg::Accel | Acceleration of the entity. | + +### setAccelerationLimit + +By using `API::setAccelerationLimit`, you can set the acceleration limit of the entity immediately. + +| Value | Type | Description | +| ------------ | ------ | --------------------------------- | +| name | string | Name of the entity. | +| acceleration | double | Acceleration limit of the entity. | + +### setAccelerationRateLimit + +By using `API::setAccelerationRateLimit`, you can set the acceleration rate limit of the entity immediately. + +| Value | Type | Description | +| ----------------- | ------ | -------------------------------------- | +| name | string | Name of the entity. | +| acceleration_rate | double | Acceleration rate limit of the entity. | + +### setDecelerationLimit + +By using `API::setDecelerationLimit`, you can set the deceleration limit of the entity immediately. + +| Value | Type | Description | +| ------------ | ------ | --------------------------------- | +| name | string | Name of the entity. | +| deceleration | double | Deceleration limit of the entity. | + +### setDecelerationRateLimit + +By using `API::setDecelerationRateLimit`, you can set the deceleration rate limit of the entity immediately. + +| Value | Type | Description | +| ----------------- | ------ | -------------------------------------- | +| name | string | Name of the entity. | +| deceleration_rate | double | Deceleration rate limit of the entity. | + +### setVelocityLimit + +By using `API::setVelocityLimit`, you can set the velocity limit of the entity immediately. + +| Value | Type | Description | +| --------------- | ------ | ----------------------------- | +| name | string | Name of the entity. | +| linear_velocity | double | Velocity limit of the entity. | diff --git a/docs/developer_guide/OpenSCENARIOSupport.md b/docs/developer_guide/OpenSCENARIOSupport.md index e71a23da9d5..89c9c99b98d 100644 --- a/docs/developer_guide/OpenSCENARIOSupport.md +++ b/docs/developer_guide/OpenSCENARIOSupport.md @@ -7,184 +7,188 @@ If you want to know about OpenSCENARIO, refer to the link below. - [ASAM OpenSCENARIO: User Guide](https://releases.asam.net/OpenSCENARIO/1.0.0/ASAM_OpenSCENARIO_BS-1-2_User-Guide_V1-0-0.html) - [OpenSCENARIO 1.0.0 XSD documentation](https://releases.asam.net/OpenSCENARIO/1.0.0/Model-Documentation/index.html) -## Specific Features ---- +## Dialects and Extensions -### ROS 2 Launch-like substitution syntax +### Substitution Syntax -Our interpreter supports some substitution syntax of [the ROS 2 Launch system](https://design.ros2.org/articles/roslaunch_xml.html#dynamic-configuration) (a.k.a. string-interpolation). -The substitution syntax works with any attribute string in OpenSCENARIO XML. +The interpreter supports some substitution syntax of [the ROS 2 Launch system](https://design.ros2.org/articles/roslaunch_xml.html#dynamic-configuration) (a.k.a. string-interpolation). The substitution syntax works with any attribute string in OpenSCENARIO XML. -This substitution is done only once when reading the attribute. -Note that the substitution result is finalized before the simulation starts, so it is not affected by the `ParameterModifyAction`, etc. that takes effect during the simulation. +Substitution syntaxes can be nested and the substitution is performed from the innermost to the outermost in order. -The supported functions and their behavior are shown below. +> [!IMPORTANT] +> This substitution is performed only once during the reading of the attribute, so changes in parameters that occur during the simulation, such as those from `ParameterModifyAction`, do not affect the substitution. -#### `$(find-pkg-prefix )` +#### Available Syntax -Equivalent to the following description given in ROS 2 Design (unless we made a mistake in our implementation). +##### `$(find-pkg-prefix )` -> Substituted by the install prefix path of the given package. Forward and backwards slashes will be resolved to the local filesystem convention. Substitution will fail if the package cannot be found. +Substituted with the install prefix path of the given package. Forward and backwards slashes will be resolved to the local filesystem convention. Substitution will fail if the specified name is not ROS 2 package. -The package specified must be a ROS 2 package. +##### `$(var )` -#### `$(var )` +Substituted with an external representation of the value of the specified OpenSCENARIO parameter. The parameters you specify must be declared by `ParameterDeclarations` before `$ (var ...)` is written. -Replaces with an external representation of the value of the specified -OpenSCENARIO parameter. -The parameters you specify must be declared by ParameterDeclarations before -`$ (var ...)` is written. +##### `$(dirname)` -#### `$(dirname)` +Substituted with the path to the directory where the running scenario script is located. -Substitute the path to the directory where the running scenario script is located. +#### Example -#### Evaluation of nested substitution syntax +```yml +ParameterDeclarations: + ParameterDeclaration: + - name: map_name + parameterType: string + value: kashiwanoha +RoadNetwork: + LogicFile: + filepath: $(find-pkg-share $(var map_name)_map)/map/lanelet2_map.osm +``` -These substitution syntaxes can be nested in any rank. -The replacement is done from the inside to the outside of the nest. -An example is shown below. +### Scoping -``` XML - - - - - - - -``` +The OpenSCENARIO XML standard [does not define](https://releases.asam.net/OpenSCENARIO/1.0.0/ASAM_OpenSCENARIO_BS-1-2_User-Guide_V1-0-0.html#:~:text=If%20a%20reference,lookup%20is%20undefined.) what to do if the name cannot be resolved. In the interpreter, the names of the Element and Parameter are lexically scoped. -## Implementation Dependent Behaviors ---- +- If you refer to an identifier that does not exist, the simulation will stop with an error. +- If multiple identifiers with the same name are defined, the identifier reference is chosen that is closest to the lexical position where the reference occurred. +- Defining a `StoryboardElement` with the same name at the same level is treated as a syntax error (In normal lexical scoping, this should be handled by shadowing, but in scenario languages it is likely a copy-and-paste mistake). -OpenSCENARIO has the function that the detailed behavior is left to the decision of the implementation side, which is defined as "subject of a contract between simulation environment provider and scenario author". +### Command -### Scoping +OpenSCENARIO XML standard states that `CustomCommandAction` can be used to either issue a command to the simulation environment or start an external script. -The OpenSCENARIO standard does not define what to do if the name cannot be resolved, as quoted below. +For OpenSCENARIO interpreters implemented in scripting languages such as Python, this action is often implemented as a call to an external script file written in the same language as the host language. However, `scenario_simulator_v2` is implemented in C++ and we cannot simply implement such a feature. Therefore, `scenario_simulator_v2` treats the string given in `CustomCommandAction.type` as a command and executes it on a subprocess, as `sh` does. -> If a reference cannot be resolved uniquely, for example if too few name prefixes have been specified to disambiguate fully, the result of the lookup is undefined. +> [!NOTE] +> To make scenarios portable, the usage of `CustomCommandAction` should be avoided. -In our interpreter, the names of the Element and Parameter are lexically scoped. +#### Built-in Commands -- If you refer to an identifier that does not exist, the simulation will stop with an error. -- If multiple identifiers with the same name are defined, the identifier reference is chosen that is closest to the lexical position where the reference occurred. -- Defining a StoryboardElement with the same name at the same level is treated as a syntax error (In normal lexical scoping, this should be handled by shadowing, but in scenario languages it is likely a copy-and-paste mistake). +##### `FaultInjectionAction(, ...)` -### CustomCommandAction +Same as `FaultInjectionAction@v1`. -This Action is specified in the standard as follows. -> Used to either issue a command to the simulation environment or start an external script. Allows the user to activate custom actions in their simulation tool. +##### `FaultInjectionAction@v1(, ...)` -For OpenSCENARIO interpreters implemented in scripting languages such as Python, this Action is often implemented as a call to an external script file written in the same language as the host language. -However, our interpreter is implemented in C++ and we cannot simply implement such a feature. -Therefore, our interpreter treats the string given in CustomCommandAction.type as a command and executes it on a subprocess, as `sh` does. +Forward any number of event names to Autoware as ERROR level event. Events are forwarded by publishing to the `tier4_simulation_msgs::msg::SimulationEvents` type topic `/simulation/events`. In order to perform fault injection using this `CustomCommandAction`, Autoware must have a node that receives the above message types. Note that `scenario_simulator_v2` has no knowledge of the contents of the event name. In other words, what happens to Autoware by this `CustomCommandAction` depends on Autoware implementation. -For example, the `echo` command can be written as follows: -``` XML - - Hello, world! - +##### `FaultInjectionAction@v2(, )` + +Forwards a single event to Autoware with the specified error level. Same as `FaultInjectionAction@v1` except that instead of specifying an error level, only one event can be specified at a time. Available error levels are `OK`, `WARN`, `ERROR` and `STALE`. + +##### `PseudoTrafficSignalDetectorConfidenceSetAction@v1(, )` + +Set a confidence value for traffic light topic. This action sets the confidence value to all traffic light bulbs of specified traffic light. If you specify the traffic light by a regulatory element ID, this action sets the confidence value to all traffic lights the regulatory element refers to. + +##### `RequestToCooperateCommandAction@v1(, )` + +Send an `ACTIVATE` / `DEACTIVATE` command to the module publishing a valid request to cooperate. If the send fails, throw an exception to fail the scenario. + +##### `V2ITrafficSignalStateAction(, , )` + +`TrafficSignalStateAction` for V2I traffic signal. You can optionally specify the publish rate of the traffic signal topic, but otherwise the functionality is the same as `TrafficSignalStateAction`. + +##### `WalkStraightAction(, ...)` + +Make **pedestrian** entities walk straight without a target. + +##### `exitFailure` + +This command immediately terminates the simulation as a failure without transitioning the state of the `StoryboardElement`. See [Termination](#termination) for more details. + +##### `exitSuccess` + +This command immediately terminates the simulation as a success without transitioning the state of the `StoryboardElement`. See [Termination](#termination) for more details. + +##### `:` (do nothing) + +Actually `:` is not a built-in command but executed as a shell command, it is worth mentioning here as it achieves 'doing nothing'. `:` is known as the null command in shell scripts, and can be used as a command that does nothing in the simulator as well. + +```yml +UserDefinedAction: + CustomCommandAction: + type: ':' +``` + +#### Example + +In YAML format, `echo` command can be written as follows: + +```yml +UserDefinedAction: + CustomCommandAction: + type: 'echo Hello, world!' ``` -The string given to attribute `type` of CustomCommandAction and the string given to its content are concatenated with whitespace and passed to the subprocess. -Therefore, the following two cases have the same effect. +In XML format, the string given to attribute `type` of `CustomCommandAction` and the string given to its content are concatenated with whitespace and passed to the subprocess so following two examples have the same effect. ``` XML - - Hello, world" - + + Hello, world" + ``` ``` XML - - - + + + ``` The effect of calling a command with `CustomCommandAction` is outside the control of the interpreter. Therefore, if you call a command that has a destructive effect on the system, there is no guarantee that the scenario execution can continue normally. -**For portable scenarios, the use of CustomCommandAction should be avoided as much as possible or limited to the scope of POSIX.** +### Condition -#### NullAction +`UserDefinedValueCondition` enables simulators to import external values and compare them with a specific value. The boolean value of the comparison result can be used as a condition to control the scenario. -In particular, the following usages that achieve "do nothing action" are worth special mention. -Here, the colon (`:`) specified in the `CustomCommandAction.type` is the `sh` command is known by the name of the null-command. +#### Autoware-related Built-in Conditions -``` XML - - - -``` +`scenario_simulator_v2` uses `UserDefinedValueCondition` to control the progress of the scenario by Autoware's state. -#### Built-in commands +##### `.currentState` -| Syntax | Description | -|:------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| 
UserDefinedAction:
  CustomCommandAction:
    type: FaultInjectionAction(, ...)
| Same as FaultInjectionAction@v1. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: FaultInjectionAction@v1(, ...)
| Forward any number of event names to Autoware as `ERROR` level event. Events are forwarded by publishing to the `tier4_simulation_msgs::msg::SimulationEvents` type topic `/simulation/events`. In order to perform fault injection using this CustomCommandAction, Autoware must have a node that receives the above message types. Note that the simulator has no knowledge of the contents of the event name. In other words, what happens to Autoware by this CustomCommandAction depends on Autoware implementation. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: FaultInjectionAction@v2(, )
| Forwards a single event to Autoware with the specified error level. Same as `FaultInjectionAction@v1` except that instead of specifying an error level, only one event can be specified at a time. Available error levels are `OK`, `WARN`, `ERROR` and `STALE`. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: PseudoTrafficSignalDetectorConfidenceSetAction@v1(, )
| Set a confidence value for traffic light topic. This action sets the confidence value to all traffic light bulbs of specified traffic light. If you specify the traffic light by a regulatory element ID, this action sets the confidence value to all traffic lights the regulatory element refers to. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: RequestToCooperateCommandAction@v1(, )
| Send an `ACTIVATE` / `DEACTIVATE` command to the module publishing a valid request to cooperate. If the send fails, throw an exception to fail the scenario. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: V2ITrafficSignalStateAction(, ,    \
                                      )
| TrafficSignalStateAction for V2I traffic signal. You can optionally specify the publish rate of the traffic signal topic, but otherwise the functionality is the same as `TrafficSignalStateAction`. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: WalkStraightAction(, ...)
| Make **pedestrian** entities walk straight without a target. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: exitFailure
| Immediately terminates the simulation as a failure. | -| 
UserDefinedAction:
  CustomCommandAction:
    type: exitSuccess
| Immediately terminates the simulation as successful. | +Returns Autoware's [state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. -The termination ignores the StoryboardElement's lifecycle transition (that is, it means that `StoryboardElementStateCondition` cannot be used to prevent or detect the execution of this command). +##### `.currentMinimumRiskManeuverState.behavior` -The terminated scenario determines the final success / failure / error. +Returns Autoware's [MRM behavior](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. -**Currently, simulation results are notified by simply writing to standard output as text. This notification method is temporary and will change in the near future.** +##### `.currentMinimumRiskManeuverState.state` -The following built-in commands are debug commands for interpreter developers, not scenario creators. -It is unlikely that you will need these commands for normal scenario creation. +Returns Autoware's [MRM state](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. -| Name | Effect | -|:--------|:--------------------------------------------------------------------------------------------------| -| error | Generate internal error: Used to ensure that the simulator does not crash with an internal error. | -| sigsegv | Access a null pointer: Used to make sure the simulator does not crash with an internal error. | +##### `.currentEmergencyState` -### UserDefinedValueCondition +Returns Autoware's [emergency state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. -This condition enables us to import external values and compare them with a specific value. -The boolean value of the comparison result can be used as a condition to control the scenario. -In scenario_simulator_v2, we use `UserDefinedValueCondition` to control the progress of the scenario by Autoware's state. +##### `.currentTurnIndicatorsState` -```XML - - - -``` -#### Built-in conditions +Returns Autoware's [turn indicators state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. + +#### Other Built-in Conditions + +##### `RelativeHeadingCondition()` -Like "currentState", the conditions start with "current" return Autoware-related conditions. -And like "ego.currentState", they can specify the entity reference by prepending the name of the entity +Calculates the relative angle between the orientation of `` and the orientation of the lane on which `` is positioned. -The following built-in conditions return a string that represents the state. -See Reference for specific strings. +##### `RelativeHeadingCondition(, , )` -| Name | Syntax | Description | -|----------------------------|----------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| currentState | `.currentState` | Returns Autoware's [state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | -| currentEmergencyState | `.currentEmergencyState` | Returns Autoware's [emergency state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | -| currentTurnIndicatorsState | `.currentTurnIndicatorsState` | Returns Autoware's [turn indicators state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | -| RelativeHeadingCondition | `RelativeHeadingCondition()`
`RelativeHeadingCondition(, , )` | Returns the relative angle to the lane heading. | +Calculates the relative angle between the orientation of `` and the direction at the position specified by `` on ``. -#### External ROS 2 topic condition +#### ROS 2 Topic Condition -You can pass values from another ROS 2 node to a scenario through ROS 2 topics. -The `name` field should be filled with the name of the ROS 2 topic like below. -```XML - - - +`scenario_simulator_v2` can read values from another ROS 2 node to a scenario through ROS 2 topics. `name` field should be filled with the name of the ROS 2 topic like below. + +```yml +ByValueCondition: + UserDefinedValueCondition: + name: /count_up + rule: equalTo + value: '500' ``` The type of topic must be `tier4_simulation_msgs::msg::UserDefinedValue` type. -You can handle the following through this function. +`scenario_simulator_v2` can handle the following through this function. - Boolean - DateTime @@ -196,200 +200,669 @@ You can handle the following through this function. See [Message Definitions](https://github.com/tier4/tier4_autoware_msgs/tree/tier4/universe/tier4_simulation_msgs) for more information. -## Non-Standard Extensions ---- - -### Success/failure judgment - -Our interpreters have been developed with the intention of incorporating them into the CI / CD pipelines in autonomous driving systems. -Therefore, executing a scenario can result in success, failure, or error. -Note that "error" means a flaw in the scenario, such as a syntax error, or an internal error, not an "error in an automated driving system" such as "a vehicle accident occurred in a simulation". -In such cases, you will be notified of a "failure". - -## Supporting Status ---- - -Our OpenSCENARIO Interpreter does not currently support the full range of -OpenSCENARIO standards. - -### Actions - -| Name | Support Status | Limitations | OpenSCENARIO changes | -|:----------------------------------------------------------------------------------------|:--------------:|:-----------------------------------|----------------------| -| GlobalAction.**EnvironmentAction** | Unsupported | | | -| GlobalAction.EntityAction.**AddEntityAction** | ✔ | | | -| GlobalAction.EntityAction.**DeleteEntityAction** | ✔ | | | -| GlobalAction.ParameterAction.**ParameterSetAction** | ✔ | See [here](#parametersetaction) | deprecated from v1.2 | -| GlobalAction.ParameterAction.**ParameterModifyAction** | ✔ | No | deprecated from v1.2 | -| GlobalAction.InfrastructureAction.TrafficSignalAction.**TrafficSignalControllerAction** | ✔ | | | -| GlobalAction.InfrastructureAction.TrafficSignalAction.**TrafficSignalStateAction** | ✔ | | | -| GlobalAction.TrafficAction.**TrafficSourceAction** | Unsupported | | | -| GlobalAction.TrafficAction.**TrafficSinkAction** | Unsupported | | | -| GlobalAction.TrafficAction.**TrafficSwarmAction** | Unsupported | | | -| GlobalAction.TrafficAction.**TrafficStopAction** | Unsupported | | | -| GlobalAction.VariableAction.**VariableSetAction** | Unsupported | | created in v1.2 | -| GlobalAction.VariableAction.**VariableModifyAction** | Unsupported | | created in v1.2 | -| UserDefinedAction.**CustomCommandAction** | ✔ | See [here](#customcommandaction) | | -| PrivateAction.AppearanceAction.**AnimationAction** | Unsupported | | created in v1.2 | -| PrivateAction.AppearanceAction.**LightStateAction** | Unsupported | | created in v1.2 | -| PrivateAction.LongitudinalAction.**SpeedAction** | ✔ | See [here](#speedaction) | | -| PrivateAction.LongitudinalAction.**SpeedProfileAction** | Unsupported | | created in v1.2 | -| PrivateAction.LongitudinalAction.**LongitudinalDistanceAction** | Unsupported | | | -| PrivateAction.LateralAction.**LaneChangeAction** | ✔ | See [here](#lanechangeaction) | | -| PrivateAction.LateralAction.**LaneOffsetAction** | Unsupported | | | -| PrivateAction.LateralAction.**LateralDistanceAction** | Unsupported | | | -| PrivateAction.**VisibilityAction** | Unsupported | | | -| PrivateAction.**SynchronizeAction** | Unsupported | | | -| PrivateAction.**ActivateControllerAction** | Unsupported | | deprecated from v1.2 | -| PrivateAction.ControllerAction.**AssignControllerAction** | ✔ | | | -| PrivateAction.ControllerAction.**ActivateControllerAction** | Unsupported | | | -| PrivateAction.ControllerAction.**OverrideControllerValueAction** | Unsupported | | | -| PrivateAction.**TeleportAction** | ✔ | See [here](#teleportaction) | | -| PrivateAction.RoutingAction.**AssignRouteAction** | ✔ | | | -| PrivateAction.RoutingAction.**FollowTrajectoryAction** | ✔ | See [here](#followtrajectoryaction)| | -| PrivateAction.RoutingAction.**AcquirePositionAction** | ✔ | See [here](#acquirepositionaction) | | +#### Example + +```yml +ByValueCondition: + UserDefinedValueCondition: + name: ego.currentState + rule: equalTo + value: ARRIVED_GOAL +``` + +### Termination + +`scenario_simulator_v2` is being developed to be integrated into the CI/CD pipeline of autonomous driving systems, and it concludes simulations with a status of either `success`, `failure`, or `error`. `failure` and `error` are each caused by different factors. Specifically, `failure` is used when issues arise from the autonomous driving system itself, such as a vehicle accident occurring during the simulation. On the other hand, `error` is specifically attributed to problems on the simulator's side, such as syntax errors in the scenario file or internal errors within the simulator itself. + +Within the scenario, you can end the scenario simulation with a status of either `success` or `failure` by using `exitSuccess` and `exitFailure` from `CustomCommandAction`. + +> [!IMPORTANT] +> `exitSuccess` and `exitFailure` terminate the simulation immediately without any state transitions in the lifecycle of a `StoryboardElement`. This means there is no way to detect simulations terminated by `exitSuccess` or `exitFailure` from within the scenario using conditions like `StoryboardElementStateCondition`. + +Currently, the only way to know the result of the simulation is by viewing the status string printed to standard output. However, this method may change in the future. + +## Standards Supported by `scenario_simulator_v2` + +| Name | Supported Version | Detail | +|:---------------------------------------------|:-----------------:|:------------------------------------------:| +| AbsoluteSpeed | unimplemented | | +| AbsoluteTargetLane | 1.3 | | +| AbsoluteTargetLaneOffset | unimplemented | | +| AbsoluteTargetSpeed | 1.3 | | +| AccelerationCondition | 1.3 | | +| AcquirePositionAction | 1.3 (partial) | [detail](#AcquirePositionAction) | +| Act | 1.3 | | +| Action | 1.3 | | +| ActivateControllerAction | unimplemented | | +| Actors | 1.3 (partial) | | +| AddEntityAction | 1.3 | | +| AngleCondition | unimplemented | | +| AngleType | unimplemented | | +| AnimationAction | unimplemented | | +| AnimationFile | unimplemented | | +| AnimationState | unimplemented | | +| AnimationType | unimplemented | | +| AppearanceAction | unimplemented | | +| AssignControllerAction | 1.2 (partial) | [detail](#AssignControllerAction) | +| AssignRouteAction | 1.3 | | +| AutomaticGear | unimplemented | | +| AutomaticGearType | unimplemented | | +| Axle | 1.3 | | +| Axles | 1.3 | [detail](#Axles) | +| BoundingBox | 1.3 | | +| Brake | unimplemented | | +| BrakeInput | unimplemented | | +| ByEntityCondition | 1.3 | | +| ByObjectType | unimplemented | | +| ByType | 1.3 | | +| ByValueCondition | 1.3 (partial) | [detail](#ByValueCondition) | +| Catalog | 1.3 (partial) | [detail](#Catalog) | +| CatalogDefinition | 1.3 | | +| CatalogLocations | 1.3 | | +| CatalogReference | 1.3 | | +| Center | 1.3 | | +| CentralSwarmObject | unimplemented | | +| Clothoid | unimplemented | | +| ClothoidSpline | unimplemented | | +| ClothoidSplineSegment | unimplemented | | +| CloudState | unimplemented | | +| CollisionCondition | 1.3 (partial) | [detail](#CollisionCondition) | +| Color | unimplemented | | +| ColorCmyk | unimplemented | | +| ColorRgb | unimplemented | | +| ColorType | unimplemented | | +| ComponentAnimation | unimplemented | | +| Condition | 1.3 | | +| ConditionEdge | 1.3 (modified) | [detail](#ConditionEdge) | +| ConditionGroup | 1.3 | | +| ConnectTrailerAction | unimplemented | | +| ControlPoint | unimplemented | | +| Controller | 1.1 | [detail](#Controller) | +| ControllerAction | 1.0 | [detail](#ControllerAction) | +| ControllerCatalogLocation | 1.3 | | +| ControllerDistribution | unimplemented | | +| ControllerDistributionEntry | unimplemented | | +| ControllerType | unimplemented | | +| CoordinateSystem | 1.2 (partial) | [detail](#CoordinateSystem) | +| CustomCommandAction | 1.3 | | +| CustomContent | unimplemented | | +| DeleteEntityAction | 1.3 | | +| Deterministic | 1.3 | | +| DeterministicMultiParameterDistribution | 1.3 | | +| DeterministicMultiParameterDistributionType | 1.3 | | +| DeterministicParameterDistribution | 1.3 | | +| DeterministicSingleParameterDistribution | 1.3 | | +| DeterministicSingleParameterDistributionType | 1.3 | | +| Dimensions | 1.3 | | +| DirectionOfTravelDistribution | unimplemented | | +| DirectionalDimension | unimplemented | | +| Directory | 1.3 | | +| DisconnectTrailerAction | unimplemented | | +| DistanceCondition | 1.3 (partial) | [detail](#DistanceCondition) | +| DistributionDefinition | 1.3 | | +| DistributionRange | 1.3 (partial) | [detail](#DistributionRange) | +| DistributionSet | 1.3 | | +| DistributionSetElement | 1.3 | | +| DomeImage | 1.3 | | +| DynamicConstraints | 1.3 | | +| DynamicsDimension | 1.3 | | +| DynamicsShape | 1.3 (partial) | [detail](#DynamicsShape) | +| EndOfRoadCondition | unimplemented | | +| Entities | 1.3 (partial) | [detail](#Entities) | +| EntityAction | 1.3 | | +| EntityCondition | 1.3 (partial) | [detail](#EntityCondition) | +| EntityDistribution | unimplemented | | +| EntityDistributionEntry | unimplemented | | +| EntityObject | 1.3 (partial) | [detail](#EntityObject) | +| EntityRef | 1.3 | | +| EntitySelection | 1.3 | | +| Environment | 1.3 | | +| EnvironmentAction | 1.3 | | +| EnvironmentCatalogLocation | 1.3 | | +| Event | 1.3 | | +| ExternalObjectReference | unimplemented | | +| File | 1.3 | | +| FileHeader | 1.3 | | +| FinalSpeed | unimplemented | | +| Fog | 1.3 | | +| FollowTrajectoryAction | 1.3 | [detail](#FollowTrajectoryAction) | +| FollowingMode | 1.3 | | +| FractionalCloudCover | 1.3 | | +| Gear | unimplemented | | +| GeoPosition | unimplemented | | +| GlobalAction | 1.1 (partial) | [detail](#GlobalAction) | +| Histogram | 1.3 | | +| HistogramBin | 1.3 | | +| InRoutePosition | unimplemented | | +| InfrastructureAction | 1.3 | | +| Init | 1.3 | | +| InitActions | 1.3 | | +| Knot | unimplemented | | +| Lane | unimplemented | | +| LaneChangeAction | 1.3 (partial) | [detail](#LaneChangeAction) | +| LaneChangeTarget | 1.3 | | +| LaneOffsetAction | unimplemented | | +| LaneOffsetActionDynamics | unimplemented | | +| LaneOffsetTarget | unimplemented | | +| LanePosition | 1.3 | | +| LateralAction | 1.3 (partial) | [detail](#LateralAction) | +| LateralDisplacement | unimplemented | | +| LateralDistanceAction | unimplemented | | +| License | 1.3 | | +| LightMode | unimplemented | | +| LightState | unimplemented | | +| LightStateAction | unimplemented | | +| LightType | unimplemented | | +| LogNormalDistribution | unimplemented | | +| LongitudinalAction | 1.3 | [detail](#LongitudinalAction) | +| LongitudinalDisplacement | unimplemented | | +| LongitudinalDistanceAction | unimplemented | | +| Maneuver | 1.3 | | +| ManeuverCatalogLocation | 1.3 | | +| ManeuverGroup | 1.3 | | +| ManualGear | unimplemented | | +| MiscObject | 1.3 | | +| MiscObjectCatalogLocation | 1.3 | | +| MiscObjectCategory | 1.3 (partial) | [detail](#MiscObjectCategory) | +| ModifyRule | 1.1 | [detail](#ModifyRule) | +| MonitorDeclaration | unimplemented | | +| MonitorDeclarations | unimplemented | | +| None | 1.3 | | +| NormalDistribution | 1.3 | | +| Nurbs | unimplemented | | +| ObjectController | 1.3 | [detail](#ObjectController) | +| ObjectType | 1.3 | | +| OffroadCondition | unimplemented | | +| OpenScenario | 1.3 | | +| OpenScenarioCategory | 1.3 (modified) | [detail](#OpenScenarioCategory) | +| Orientation | 1.3 | | +| OverrideBrakeAction | unimplemented | | +| OverrideClutchAction | unimplemented | | +| OverrideControllerValueAction | unimplemented | | +| OverrideGearAction | unimplemented | | +| OverrideParkingBrakeAction | unimplemented | | +| OverrideSteeringWheelAction | unimplemented | | +| OverrideThrottleAction | unimplemented | | +| ParameterAction | 1.1 | [detail](#ParameterAction) | +| ParameterAddValueRule | 1.1 | [detail](#ParameterAddValueRule) | +| ParameterAssignment | 1.3 | | +| ParameterCondition | 1.3 | | +| ParameterDeclaration | 1.3 | | +| ParameterModifyAction | 1.1 | [detail](#ParameterModifyAction) | +| ParameterMultiplyByValueRule | 1.1 | [detail](#ParameterMultiplyByValueRule) | +| ParameterSetAction | 1.1 | [detail](#ParameterSetAction) | +| ParameterType | 1.3 | [detail](#ParameterType) | +| ParameterValueDistribution | 1.3 | | +| ParameterValueDistributionDefinition | 1.3 | | +| ParameterValueSet | 1.3 | | +| Pedestrian | 1.3 (partial) | [detail](#Pedestrian) | +| PedestrianAnimation | unimplemented | | +| PedestrianCatalogLocation | 1.3 | | +| PedestrianCategory | 1.3 (partial) | [detail](#PedestrianCategory) | +| PedestrianGesture | unimplemented | | +| PedestrianGestureType | unimplemented | | +| PedestrianMotionType | unimplemented | | +| Performance | 1.3 | | +| Phase | 1.1 | [detail](#Phase) | +| PoissonDistribution | 1.3 | | +| Polygon | unimplemented | | +| Polyline | 1.3 | | +| Position | 1.3 (partial) | [detail](#Position) | +| PositionInLaneCoordinates | unimplemented | | +| PositionInRoadCoordinates | unimplemented | | +| PositionOfCurrentEntity | unimplemented | | +| Precipitation | 1.3 | [detail](#Precipitation) | +| PrecipitationType | 1.3 | | +| Priority | 1.1 | [detail](#Priority) | +| Private | 1.3 | | +| PrivateAction | 1.3 | [detail](#PrivateAction) | +| ProbabilityDistributionSet | 1.3 | | +| ProbabilityDistributionSetElement | 1.3 | | +| Properties | 1.3 (partial) | [detail](#Properties) | +| Property | 1.3 | | +| RandomRouteAction | unimplemented | | +| Range | 1.3 | | +| ReachPositionCondition | 1.1 | [detail](#ReachPositionCondition) | +| ReferenceContext | 1.3 (partial) | [detail](#ReferenceContext) | +| RelativeAngleCondition | unimplemented | | +| RelativeClearanceCondition | 1.3 (partial) | [detail](#RelativeClearanceCondition) | +| RelativeDistanceCondition | 1.3 (partial) | [detail](#RelativeDistanceCondition) | +| RelativeDistanceType | 1.3 | [detail](#RelativeDistanceType) | +| RelativeLanePosition | unimplemented | | +| RelativeLaneRange | 1.3 | | +| RelativeObjectPosition | 1.3 | | +| RelativeRoadPosition | unimplemented | | +| RelativeSpeedCondition | unimplemented | | +| RelativeSpeedToMaster | unimplemented | | +| RelativeTargetLane | 1.3 | | +| RelativeTargetLaneOffset | unimplemented | | +| RelativeTargetSpeed | 1.3 | | +| RelativeWorldPosition | 1.3 | | +| RoadCondition | 1.3 | | +| RoadCursor | unimplemented | | +| RoadNetwork | 1.3 (partial) | [detail](#RoadNetwork) | +| RoadPosition | unimplemented | | +| RoadRange | unimplemented | | +| Role | unimplemented | | +| Route | 1.3 | | +| RouteCatalogLocation | 1.3 | | +| RoutePosition | unimplemented | | +| RouteRef | unimplemented | | +| RouteStrategy | 1.3 (partial) | [detail](#RouteStrategy) | +| RoutingAction | 1.2 | [detail](#RoutingAction) | +| RoutingAlgorithm | 1.3 | | +| Rule | 1.3 | | +| ScenarioDefinition | 1.1 | [detail](#ScenarioDefinition) | +| ScenarioObject | 1.3 | | +| ScenarioObjectTemplate | unimplemented | | +| SelectedEntities | 1.3 | | +| SensorReference | unimplemented | | +| SensorReferenceSet | unimplemented | | +| SetMonitorAction | unimplemented | | +| Shape | 1.2 | [detail](#Shape) | +| SimulationTimeCondition | 1.3 | | +| SpeedAction | 1.3 (partial) | [detail](#SpeedAction) | +| SpeedActionTarget | 1.3 | | +| SpeedCondition | 1.3 (partial) | [detail](#SpeedCondition) | +| SpeedProfileAction | 1.3 | | +| SpeedProfileEntry | 1.3 | | +| SpeedTargetValueType | 1.3 | | +| StandStillCondition | 1.3 | | +| SteadyState | unimplemented | | +| Stochastic | 1.3 | | +| StochasticDistribution | 1.3 | | +| StochasticDistributionType | 1.2 | [detail](#StochasticDistributionType) | +| Story | 1.3 | | +| Storyboard | 1.3 | | +| StoryboardElementState | 1.3 | | +| StoryboardElementStateCondition | 1.3 (modified) | [detail](#StoryboardElementStateCondition) | +| StoryboardElementType | 1.3 | | +| Sun | 1.3 | [detail](#Sun) | +| SynchronizeAction | unimplemented | | +| TargetDistanceSteadyState | unimplemented | | +| TargetTimeSteadyState | unimplemented | | +| TeleportAction | 1.3 | [detail](#TeleportAction) | +| TimeHeadwayCondition | 1.0 | [detail](#TimeHeadwayCondition) | +| TimeOfDay | 1.3 | | +| TimeOfDayCondition | unimplemented | | +| TimeReference | 1.3 | | +| TimeToCollisionCondition | unimplemented | | +| TimeToCollisionConditionTarget | unimplemented | | +| Timing | 1.3 | | +| TrafficAction | unimplemented | | +| TrafficArea | unimplemented | | +| TrafficAreaAction | unimplemented | | +| TrafficDefinition | unimplemented | | +| TrafficDistribution | unimplemented | | +| TrafficDistributionEntry | unimplemented | | +| TrafficSignalAction | 1.3 | | +| TrafficSignalCondition | 1.3 | | +| TrafficSignalController | 1.3 | | +| TrafficSignalControllerAction | 1.3 | | +| TrafficSignalControllerCondition | 1.3 | | +| TrafficSignalGroupState | unimplemented | | +| TrafficSignalState | 1.3 | | +| TrafficSignalStateAction | 1.3 | | +| TrafficSinkAction | unimplemented | | +| TrafficSourceAction | unimplemented | | +| TrafficStopAction | unimplemented | | +| TrafficSwarmAction | unimplemented | | +| Trailer | unimplemented | | +| TrailerAction | unimplemented | | +| TrailerCoupler | unimplemented | | +| TrailerHitch | unimplemented | | +| Trajectory | 1.3 | | +| TrajectoryCatalogLocation | 1.3 | | +| TrajectoryFollowingMode | 1.3 | | +| TrajectoryPosition | unimplemented | | +| TrajectoryRef | 1.3 | | +| TransitionDynamics | 1.1 | [detail](#TransitionDynamics) | +| TraveledDistanceCondition | unimplemented | | +| Trigger | 1.3 | | +| TriggeringEntities | 1.3 | | +| TriggeringEntitiesRule | 1.3 (modified) | [detail](#TriggeringEntitiesRule) | +| UniformDistribution | 1.3 | | +| UsedArea | unimplemented | | +| UserDefinedAction | 1.3 | | +| UserDefinedAnimation | unimplemented | | +| UserDefinedComponent | unimplemented | | +| UserDefinedDistribution | unimplemented | | +| UserDefinedLight | unimplemented | | +| UserDefinedValueCondition | 1.3 | | +| ValueConstraint | 1.3 | | +| ValueConstraintGroup | 1.3 | | +| ValueSetDistribution | 1.3 | | +| VariableAction | unimplemented | | +| VariableAddValueRule | unimplemented | | +| VariableCondition | unimplemented | | +| VariableDeclaration | unimplemented | | +| VariableDeclarations | unimplemented | | +| VariableModifyAction | unimplemented | | +| VariableModifyRule | unimplemented | | +| VariableMultiplyByValueRule | unimplemented | | +| VariableSetAction | unimplemented | | +| Vehicle | 1.1 (partial) | [detail](#Vehicle) | +| VehicleCatalogLocation | 1.3 | | +| VehicleCategory | 1.3 | | +| VehicleCategoryDistribution | unimplemented | | +| VehicleCategoryDistributionEntry | unimplemented | | +| VehicleComponent | unimplemented | | +| VehicleComponentType | unimplemented | | +| VehicleLight | unimplemented | | +| VehicleLightType | unimplemented | | +| VehicleRoleDistribution | unimplemented | | +| VehicleRoleDistributionEntry | unimplemented | | +| Vertex | 1.3 | | +| VisibilityAction | unimplemented | | +| Waypoint | 1.3 | | +| Weather | 1.3 | | +| Wetness | 1.3 | | +| Wind | 1.3 | | +| WorldPosition | 1.3 | | + +### Details + +#### AcquirePositionAction + +- Property `Position` of types `RoadPosition`, `RelativeRoadPosition`, `RelativeLanePosition`, `RoutePosition`, `GeoPosition`, and `TrajectoryPosition` are **not** supported. + +#### AssignControllerAction + +- Properties `activateAnimation`, `activateLateral`, `activateLighting`, and `activateLongitudinal` are ignored. + - The simulator behaves as if these properties are `false`. +- Property `ObjectController` created in version 1.3 is **not** supported. +- Properties `Controller` and `CatalogReference` deprecated in version 1.3 are supported. + +#### Axles + +- Property `RearAxle` is made mandatory in version 1.3, but still can be omitted like specified in version 1.2. + +#### ByValueCondition + +- Properties `TimeOfDayCondition` and `VariableCondition` are **not** supported. + +#### Catalog + +- Property `Trajectory` is ignored. + +#### CollisionCondition + +- Property `ByType` is **not** supported. + +#### ConditionEdge + +- Enumeration literal `sticky` is added as TIER IV extension. + +#### Controller + +- Property `controllerType` created in version 1.2 is ignored. + +#### ControllerAction + +- Property `OverrideControllerValueAction` is ignored. +- Property `ActivateControllerAction` created in version 1.1 is ignored. + +#### CoordinateSystem + +- Enumeration literals `road` and `trajectory` are **not** supported. +- Enumeration literal `world` created in version 1.3 is **not** supported. + +#### DistanceCondition + +- Property `alongRoute` deprecated in version 1.1 is **not** supported. +- Property `Position` of types `RoadPosition`, `RelativeRoadPosition`, `RelativeLanePosition`, `RoutePosition`, `GeoPosition`, and `TrajectoryPosition` are **not** supported. +- Not all combinations of properties for distance calculation are supported. Supported combinations are listed below: + | coordinateSystem | relativeDistanceType | routingAlgorithm | freespace | + |:----------------:|:--------------------:|:----------------:|:---------:| + | entity | euclidianDistance | undefined | false | + | entity | euclidianDistance | undefined | true | + | entity | lateral | undefined | false | + | entity | lateral | undefined | true | + | entity | longitudinal | undefined | false | + | entity | longitudinal | undefined | true | + | lane | lateral | undefined | false | + | lane | lateral | undefined | true | + | lane | longitudinal | undefined | false | + | lane | longitudinal | undefined | true | + | lane | lateral | shortest | false | + | lane | lateral | shortest | true | + | lane | longitudinal | shortest | false | + | lane | longitudinal | shortest | true | + +#### DistributionRange + +- Property `stepWidth` is ignored. + +#### DynamicsShape + +- Enumeration literal `sinusoidal` is **not** supported. + +#### Entities + +- Property `EntitySelection` is **not** supported. + +#### EntityCondition + +- Properties `EndOfRoadCondition`, `OffroadCondition`, `TimeToCollisionCondition`, `RelativeDistanceCondition`, `TraveledDistanceCondition`, `AngleCondition`, and `RelativeAngleCondition` are **not** supported. +- Property `ReachPositionCondition` deprecated in version 1.2 is still supported. + +#### EntityObject + +- Property `ExternalObjectReference` is **not** supported. + +#### FollowTrajectoryAction + +- Properties `Trajectory` and `CatalogReference` deprecated in version 1.1 are ignored. +- Property `TrajectoryRef` of type `Clothoid` and `Nurbs` are **not** supported. + +#### GlobalAction + +- Properties `TrafficAction` and `VariableAction` are not supported. +- Property `ParameterAction` deprecated in version 1.2 is still supported. +- Property `SetMonitorAction` created in version 1.3 is **not** supported. + +#### LaneChangeAction + +- Specifying `step` for `LaneChangeActionDynamics.dynamicsDimension` is **not** supported. + - Simulator may lead to an undefined behavior if `step` is specified. + +#### LateralAction + +- Properties `LaneOffsetAction` and `LateralDistanceAction` are **not** supported. + +#### LongitudinalAction + +- Property `LongitudinalDistanceAction` is **not** supported. + +#### MiscObjectCategory + +- Enumeration literals `barrier`, `building`, `crosswalk`, `gantry`, `none`, `parkingSpace`, `patch`, `pole`, `roadMark`, `soundBarrier`, `streetLamp`, `trafficIsland`, `tree`, and `vegetation` are **not** supported. +- Enumeration literal `wind` deprecated in version 1.1 is **not** supported. + +#### ModifyRule + +- Class `ModifyRule` deprecated in version 1.3 is still supported. + +#### ObjectController + +- Property `name` is ignored. + +#### OpenScenarioCategory + +- The simulator reads `Storyboard` in XML as property `ScenarioDefinition` +- The simulator reads `Catalog` in XML as property `CatalogDefinition` +- The simulator reads `ParameterValueDistribution` in XML as property `ParameterValueDistributionDefinition` + +#### ParameterAction + +- Class `ParameterAction` deprecated in version 1.2 is still supported. + +#### ParameterAddValueRule + +- Class `ParameterAddValueRule` deprecated in version 1.2 is still supported. + +#### ParameterModifyAction + +- Class `ParameterModifyAction` deprecated in version 1.2 is still supported. + +#### ParameterMultiplyByValueRule + +- Class `ParameterMultiplyByValueRule` deprecated in version 1.2 is still supported. + +#### ParameterSetAction + +- Class `ParameterSetAction` deprecated in version 1.2 is still supported. +- Class `ParameterSetAction` cannot handle parameters of type `DataTime`. + +#### ParameterType + +- Enumeration literal `integer` deprecated in version 1.2 is **not** supported. + +#### Pedestrian + +- Property `role` is ignored. + - The simulator does not take into account `role`. +- Property `model` deprecated version 1.1 is ignored but mandatory. + - Maybe this is simulator bug and need to be fixed. + +#### PedestrianCategory + +- Enumeration literals `wheelchair` and `animal` are **not** supported. + +#### Phase + +- Property `TrafficSignalGroupState` created in version 1.2 is ignored. + +#### Position + +- Properties `RoadPosition`, `RelativeRoadPosition`, `RelativeLanePosition`, `RoutePosition`, `GeoPosition`, and `TrajectoryPosition` are **not** supported. + +#### Precipitation + +- Property `intensity` deprecated in version 1.1 is still supported. + +#### Priority + +- Enumeration literal `override` created in version 1.2 is **not** supported. +- Enumeration literal `overwrite` deprecated in version 1.2 is supported. + +#### PrivateAction + +- Property `VisibilityAction`, `SynchronizeAction` and `ActivateControllerAction` are **not** supported. + +#### Properties + +- Property `CustomContent` is ignored. + - The simulator does not take into account `CustomContent`. + +#### ReachPositionCondition + +- Class `ReachPositionCondition` deprecated in version 1.2 is still supported. +- Property `Position` of types `RoadPosition`, `RelativeRoadPosition`, `RelativeLanePosition`, `RoutePosition`, `GeoPosition`, and `TrajectoryPosition` are **not** supported. + +#### ReferenceContext + +- Enumeration literal `absolute` is **not** supported. + +#### RelativeClearanceCondition + +- Property `oppositeLanes` is ignored. + - The simulator behaves as if `oppositeLanes` is `false`. -### Conditions +#### RelativeDistanceCondition -| Name | Support Status | Limitations | OpenSCENARIO changes | -|:----------------------------------------------------------------|:--------------:|:---------------------------------------------|----------------------| -| ByEntityCondition.EntityCondition.**EndOfRoadCondition** | Unsupported | | | -| ByEntityCondition.EntityCondition.**CollisionCondition** | ✔ | See [here](#collisioncondition) | | -| ByEntityCondition.EntityCondition.**OffroadCondition** | Unsupported | | | -| ByEntityCondition.EntityCondition.**TimeHeadwayCondition** | ✔ | See [here](#timeheadwaycondition) | | -| ByEntityCondition.EntityCondition.**TimeToCollisionCondition** | Unsupported | | | -| ByEntityCondition.EntityCondition.**AccelerationCondition** | ✔ | No | | -| ByEntityCondition.EntityCondition.**StandStillCondition** | ✔ | No | | -| ByEntityCondition.EntityCondition.**SpeedCondition** | ✔ | No | | -| ByEntityCondition.EntityCondition.**RelativeSpeedCondition** | Unsupported | | | -| ByEntityCondition.EntityCondition.**TraveledDistanceCondition** | Unsupported | | | -| ByEntityCondition.EntityCondition.**ReachPositionCondition** | ✔ | See [here](#reachpositioncondition) | deprecated from v1.2 | -| ByEntityCondition.EntityCondition.**DistanceCondition** | ✔ | See [here](#distancecondition) | | -| ByEntityCondition.EntityCondition.**RelativeDistanceCondition** | ✔ | | | -| ByValueCondition.**ParameterCondition** | ✔ | | | -| ByValueCondition.**TimeOfDayCondition** | Unsupported | | | -| ByValueCondition.**SimulationTimeCondition** | ✔ | No | | -| ByValueCondition.**StoryboardElementStateCondition** | ✔ | See [here](#storyboardelementstatecondition) | | -| ByValueCondition.**UserDefinedValueCondition** | ✔ | | | -| ByValueCondition.**TrafficSignalCondition** | ✔ | | | -| ByValueCondition.**TrafficSignalControllerCondition** | ✔ | | | -| ByValueCondition.**VariableCondition** | Unsupported | | created in v1.2 | +- Property `Position` of types `RoadPosition`, `RelativeRoadPosition`, `RelativeLanePosition`, `RoutePosition`, `GeoPosition`, and `TrajectoryPosition` are **not** supported. +- Not all combinations of properties for distance calculation are supported. Supported combinations are listed below: + | coordinateSystem | relativeDistanceType | routingAlgorithm | freespace | + |:----------------:|:--------------------:|:----------------:|:---------:| + | entity | euclidianDistance | undefined | false | + | entity | euclidianDistance | undefined | true | + | entity | lateral | undefined | false | + | entity | lateral | undefined | true | + | entity | longitudinal | undefined | false | + | entity | longitudinal | undefined | true | + | lane | lateral | undefined | false | + | lane | lateral | undefined | true | + | lane | longitudinal | undefined | false | + | lane | longitudinal | undefined | true | + | lane | lateral | shortest | false | + | lane | lateral | shortest | true | + | lane | longitudinal | shortest | false | + | lane | longitudinal | shortest | true | -## Limitations +#### RelativeDistanceType -### ParameterSetAction +- Enumeration literal `cartesianDistance` deprecated in version 1.1 is **not** supported. -- Currently, ParameterSetAction cannot handle `dateTime` type parameters. +#### RoadNetwork -### CustomCommandAction -#### WalkStraightAction -- This action is a temporary feature until `FollowTrajectoryAction` is implemented. -- This action cannot be used in combination with `AcquirePositionAction` because `WalkStraightAction` just makes a pedestrian NPC go straight without a destination. +- Property `UsedArea` is ignored. +#### RouteStrategy -### SpeedAction +- Enumeration literals `fastest`, `leastIntersections` and `random` are **not** supported. -- The implementation of type [TransitionDynamics](#transitiondynamics) for element `SpeedActionDynamics` is incomplete and - **SpeedActionDynamics.dynamicsDimension is ignored**. +#### RoutingAction -### LaneChangeAction +- Property `RandomRouteAction` created in version 1.3 is **not** supported. -- The implementation of type [TransitionDynamics](#transitiondynamics) for element `LaneChangeActionDynamics` and type [LaneChangeTarget](#lanechangetarget) for element `LaneChangeTarget` are incomplete. +#### ScenarioDefinition -### TeleportAction - -- Currently, **only LanePosition** can be specified for element of - TeleportAction. - -### FollowTrajectoryAction -- Currently, the action handles only "[followingMode](https://www.asam.net/static_downloads/ASAM_OpenSCENARIO_V1.2.0_Model_Documentation/modelDocumentation/content/FollowingMode.html)" attribute set to `position` mode. - -| followingMode | Status | -|:--------------|:-----------:| -| position | ✔ | -| follow | Unsupported | - -- Currently, the action only supports [Trajectory](https://www.asam.net/static_downloads/ASAM_OpenSCENARIO_V1.2.0_Model_Documentation/modelDocumentation/content/Trajectory.html) with a [Polyline](https://www.asam.net/static_downloads/ASAM_OpenSCENARIO_V1.2.0_Model_Documentation/modelDocumentation/content/Shape.html) shape. - -| Element | Status | -|:---------|:----------:| -| Polyline | ✔ | -| Clothoid | Unsupported| -| Nurbs | Unsupported| - -### AcquirePositionAction - -- Currently, **only LanePosition** can be specified for element of - AcquirePositionAction. - -### CollisionCondition +- Property `VariableDeclarations` created in version 1.2 is ignored. +- Property `MonitorDeclarations` created in version 1.3 is ignored. -- Currently, **only EntityRef** can be specified for element of - CollisionCondition. +#### Shape -### TimeHeadwayCondition +- Properties `Clothoid` and `Nurbs` are **not** supported. +- Property `ClothoidSpline` created in version 1.3 is **not** supported. -- Currently, the values of attribute "freespace" and "alongRoute" are ignored and always behave as if freespace="false" and alongRoute="true" were specified. +#### SpeedAction -### ReachPositionCondition +- Specifying `time` or `distance` for `SpeedActionDynamics.dynamicsDimension` is **not** supported. + - Simulator may lead to an undefined behavior if `time` or `distance` is specified. +- Specifying `cubic` for `SpeedActionDynamics.dynamicsShape` is **not** supported. + - Simulator may lead to an undefined behavior if `cubic` is specified. -- Currently, **only LanePosition and WorldPosition** can be specified for the element of ReachPositionCondition. +#### SpeedCondition -### DistanceCondition +- Property `direction` is ignored. + - The simulator behaves as if `direction` is not given. -- Currently, the values of attribute "alongRoute" is ignored and always behave as if alongRoute="false" was specified. -- Currently, **only LanePosition and WorldPosition** can be specified for the element of Position of DistanceCondition. +#### StochasticDistributionType -### StoryboardElementStateCondition +- Property `LogNormalDistribution` created in version 1.3 is **not** supported. -- Currently, a feature called "name prefix" (in [OpenSCENARIO User Guide 3.1.2. Naming](https://releases.asam.net/OpenSCENARIO/1.0.0/ASAM_OpenSCENARIO_BS-1-2_User-Guide_V1-0-0.html#_general_concepts)) is unsupported. +#### StoryboardElementStateCondition -Instead, our interpreter implements lexical scoping. -See also section [Scoping](#scoping). +- [name prefix](https://releases.asam.net/OpenSCENARIO/1.0.0/ASAM_OpenSCENARIO_BS-1-2_User-Guide_V1-0-0.html#:~:text=A%20name%20prefix%20,new%20name%20reference.) in OpenSCENARIO User Guide 3.1.2. is **not** supported. + - The interpreter uses lexical scope instead. See [Scoping](#Scoping) for more details. -### TransitionDynamics +#### Sun -- The implementation of type [DynamicsShape](#dynamicsshape) for attribute dynamicsShape is incomplete. +- Property `intensity` deprecated in version 1.2 is still supported. -| Name | Type | Status | -|:------------------|:--------------------------------|:----------:| -| dynamicsShape | [DynamicsShape](#dynamicsshape) | Incomplete | -| value | Double | ✔ | -| dynamicsDimension | DynamicsDimension | ✔ | +#### TeleportAction -### DynamicsShape +- Property `Position` of types `RoadPosition`, `RelativeRoadPosition`, `RelativeLanePosition`, `RoutePosition`, `GeoPosition`, and `TrajectoryPosition` are **not** supported. -- Currently, only `linear` and `step` are implemented for values of this enumeration. - If you specify `cubic` and `sinusoidal`, you will get an ImplementationFault. +#### TimeHeadwayCondition -| Value | Status | -|:-----------|:-----------:| -| linear | ✔ | -| cubic | Unsupported | -| sinusoidal | Unsupported | -| step | ✔ | +- Properties `coordinateSystem` and `relativeDistanceType` created in version 1.1 is ignored. +- Property `alongRoute` deprecated in version 1.1 is ignored. +- Property `freespace` is ignored. + - The simulator behaves as if `freespace` is `false`. -### LaneChangeTarget +#### TransitionDynamics -- Currently, only AbsoluteTargetLane is implemented for element of this type. - If you specify RelativeTargetLane, you will get a SyntaxError. +- Property `followingMode` created in version 1.2 is ignored. -| Element | Status | -|:-------------------|:-----------:| -| AbsoluteTargetLane | ✔ | -| RelativeTargetLane | Unsupported | +#### TriggeringEntitiesRule -### Position +- Enumeration literal `none` is added as TIER IV extension. -- Currently, only WorldPosition and LanePosition are implemented for an element of this type. +#### Vehicle -| Element | Status | -|:-----------------------|:-----------:| -| WorldPosition | ✔ | -| RelativeWorldPosition | Unsupported | -| RelativeObjectPosition | ✔ | -| RoadPosition | Unsupported | -| RelativeRoadPosition | Unsupported | -| LanePosition | ✔ | -| RelativeLanePosition | Unsupported | -| RoutePosition | Unsupported | +- Property `role` created in version 1.1 is ignored. + - The simulator does not take into account `role`. +- Property `mass` is ignored. + - The simulator behaves as if `mass` is not given. diff --git a/docs/developer_guide/images/Longitudinal_control/synchronizedAction.png b/docs/developer_guide/images/Longitudinal_control/synchronizedAction.png new file mode 100644 index 00000000000..7ebcf6436c0 Binary files /dev/null and b/docs/developer_guide/images/Longitudinal_control/synchronizedAction.png differ diff --git a/docs/image/cpp_scenario_launch.gif b/docs/image/cpp_scenario_launch.gif new file mode 100644 index 00000000000..fd07b2070da Binary files /dev/null and b/docs/image/cpp_scenario_launch.gif differ diff --git a/docs/image/cpp_scenario_result.png b/docs/image/cpp_scenario_result.png new file mode 100644 index 00000000000..2fec00f5de4 Binary files /dev/null and b/docs/image/cpp_scenario_result.png differ diff --git a/docs/image/locale_verification.png b/docs/image/locale_verification.png new file mode 100644 index 00000000000..2f2e475814b Binary files /dev/null and b/docs/image/locale_verification.png differ diff --git a/docs/image/random_test_runner_launch.gif b/docs/image/random_test_runner_launch.gif new file mode 100644 index 00000000000..f6101db1b09 Binary files /dev/null and b/docs/image/random_test_runner_launch.gif differ diff --git a/docs/image/random_test_runner_result.png b/docs/image/random_test_runner_result.png new file mode 100644 index 00000000000..bb449055c92 Binary files /dev/null and b/docs/image/random_test_runner_result.png differ diff --git a/docs/image/realtime_factor/panel.png b/docs/image/realtime_factor/panel.png new file mode 100644 index 00000000000..46d25a6333b Binary files /dev/null and b/docs/image/realtime_factor/panel.png differ diff --git a/docs/image/realtime_factor/slider.png b/docs/image/realtime_factor/slider.png new file mode 100644 index 00000000000..90e50131323 Binary files /dev/null and b/docs/image/realtime_factor/slider.png differ diff --git a/docs/image/realtime_factor/video.mp4 b/docs/image/realtime_factor/video.mp4 new file mode 100644 index 00000000000..52f487d9484 Binary files /dev/null and b/docs/image/realtime_factor/video.mp4 differ diff --git a/docs/image/scenario_test_runner_launch.gif b/docs/image/scenario_test_runner_launch.gif new file mode 100644 index 00000000000..d8ecef1eb52 Binary files /dev/null and b/docs/image/scenario_test_runner_launch.gif differ diff --git a/docs/image/scenario_test_runner_result.png b/docs/image/scenario_test_runner_result.png new file mode 100644 index 00000000000..6c3be0c43ed Binary files /dev/null and b/docs/image/scenario_test_runner_result.png differ diff --git a/docs/image/ss2_autoware_build_result.png b/docs/image/ss2_autoware_build_result.png new file mode 100644 index 00000000000..d204a6acc46 Binary files /dev/null and b/docs/image/ss2_autoware_build_result.png differ diff --git a/docs/image/ss2_build_result.png b/docs/image/ss2_build_result.png new file mode 100644 index 00000000000..afc214dc1ae Binary files /dev/null and b/docs/image/ss2_build_result.png differ diff --git a/docs/user_guide/BuildInstructions.md b/docs/user_guide/BuildInstructions.md index f2829a40571..9ff0931d107 100644 --- a/docs/user_guide/BuildInstructions.md +++ b/docs/user_guide/BuildInstructions.md @@ -1,51 +1,96 @@ # Build Instructions -This setup instruction is based on ROS 2 humble. +These setup instructions guide you through the process of building **Scenario Simulator v2**, which currently supports **ROS 2 Humble Hawksbill**. ## Setup ROS 2 environment -This framework only supports ROS 2 Galactic Geochelone now. (We are planning to support ROS 2 Humble Hawksbill) -You can install Galactic by executing the command below in your terminal. +1. Configure the locale. + Ensure that your system is configured with a locale that supports UTF-8. + + ```bash + sudo apt update && sudo apt install locales + sudo locale-gen en_US en_US.UTF-8 + sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 + export LANG=en_US.UTF-8 + ``` + + Verify that the locale is properly set: + ```bash + locale + ``` + ![Locale Verification](../image/locale_verification.png) +
*The expected result after running the `locale` command should look like this, with `LANG` set to `en_US.UTF-8` by default.* + +3. Enable the Ubuntu Universe repository. + ```bash + sudo apt install software-properties-common + sudo add-apt-repository universe + ``` + +4. Add the ROS 2 GPG key. + ```bash + sudo apt update && sudo apt install curl -y + sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + ``` + +5. Add the ROS 2 repository to the system's package sources list. + ```bash + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + ``` + +6. Install ROS 2 Humble Desktop. + ```bash + sudo apt update + sudo apt install ros-humble-desktop + ``` + +7. Install development tools. + ```bash + sudo apt install ros-dev-tools + ``` +!!! info + For more detailed information, refer to the [ROS 2 Humble Installation Guide](https://docs.ros.org/en/humble/Installation.html). -```bash -sudo apt update -sudo apt install locales -sudo locale-gen en_US en_US.UTF-8 -sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 -export LANG=en_US.UTF-8 -sudo apt update -sudo apt install curl gnupg2 lsb-release -curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - -sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' -sudo apt update -sudo apt install ros-humble-desktop -source /opt/ros/humble/setup.bash -sudo apt install -y python3-pip python3-rosdep2 python3-vcstool python3-colcon-common-extensions -rosdep update -``` -reference : ## Setup workspace -```bash -mkdir -p ~/scenario_simulator_ws/src -cd ~/scenario_simulator_ws/src -git clone https://github.com/tier4/scenario_simulator_v2.git -# These lines are necessary right now, but it will be removed in the near future -cd scenario_simulator_v2 -# This script clones the part of the source codes in Autoware and add it to the workspace -vcs import external < dependency_humble.repos -``` +1. Create the workspace directory. + ```bash + mkdir -p ~/scenario_simulator_ws/src + cd ~/scenario_simulator_ws/src + ``` + +2. Clone the Scenario Simulator repository. + ```bash + git clone https://github.com/tier4/scenario_simulator_v2.git + cd scenario_simulator_v2 + ``` + +3. Import Autoware dependencies. + ```bash + vcs import external < dependency_humble.repos + ``` ## Install dependencies via rosdep -```bash -cd ~/scenario_simulator_ws -source /opt/ros/humble/setup.bash -rosdep install -iry --from-paths src/scenario_simulator_v2 --rosdistro humble -``` +1. Move to the workspace directory. + ```bash + cd ~/scenario_simulator_ws + ``` -## Build scenario_simulator_v2 +2. Source the ROS 2 environment. + ```bash + source /opt/ros/humble/setup.bash + ``` +3. Install all required dependencies. + ```bash + rosdep install -iry --from-paths src/scenario_simulator_v2 --rosdistro humble + ``` + +## Build scenario_simulator_v2 +To build **Scenario Simulator v2**, run this command: ```bash colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release ``` +![Build success](../image/ss2_build_result.png) +*As a result of running the `colcon build` command, all packages should be built successfully.* diff --git a/docs/user_guide/QuickStart.md b/docs/user_guide/QuickStart.md index 1203f12df49..f42ad70ad20 100644 --- a/docs/user_guide/QuickStart.md +++ b/docs/user_guide/QuickStart.md @@ -1,95 +1,110 @@ -# Quick Start -Scenario simulator with Autoware Universe
-This document contains step-by-step instruction on how to build and run [AWF Autoware Core/Universe](https://github.com/autowarefoundation/autoware) with `scenario_simulator_v2`. +## Quick Start + +### Scenario Simulator v2 with Autoware Universe + +This guide provides step-by-step instructions for building and running **Scenario Simulator v2** alongside [**Autoware Universe**](https://github.com/autowarefoundation/autoware). It covers the required setup and prerequisites. ## Prerequisites 1. Ubuntu 22.04 machine -3. ROS 2 Humble Hawksbill desktop version [installed](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) +2. ROS 2 Humble Hawksbill desktop version [installed](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html) ## How to build -1. Clone the Autoware Core/Universe repository: - ```bash - git clone git@github.com:autowarefoundation/autoware.git - ``` -2. Navigate to the source directory: - ```bash - cd autoware - mkdir src - ``` -3. Import Autoware and Simulator dependencies: - ```bash - vcs import src < autoware.repos - vcs import src < simulator.repos - ``` -4. Install dependencies for Autoware Core/Universe - ```bash - ./setup-dev-env.sh - ``` -[//]: # ( > Note. This step is only required during first installation/usage of Autoware Core/Universe) + 1. Clone the Autoware Universe repository. + ```bash + git clone git@github.com:autowarefoundation/autoware.git + ``` -[//]: # ( > ) + 2. Move to the workspace directory. + ```bash + cd autoware + mkdir src + ``` -[//]: # ( > Note: Before installing NVIDIA libraries, confirm and agree with the licenses.) + 3. Import Autoware and Simulator dependencies. + ```bash + vcs import src < autoware.repos + vcs import src < simulator.repos + ``` -[//]: # ( - [CUDA](https://docs.nvidia.com/cuda/eula/index.html)) + 4. Install dependencies for Autoware Universe. + ```bash + ./setup-dev-env.sh + ``` +!!! note + This step will install necessary dependencies, including ROS 2, if not already installed.
Make sure to confirm and agree with the licenses for NVIDIA libraries like [CUDA](https://docs.nvidia.com/cuda/eula/index.html), [cuDNN](https://docs.nvidia.com/deeplearning/cudnn/latest/reference/eula.html), and [TensorRT](https://docs.nvidia.com/deeplearning/tensorrt/sla/index.html). -[//]: # ( - [cuDNN](https://docs.nvidia.com/deeplearning/cudnn/sla/index.html)) + 5. Install dependent ROS packages. + ```bash + source /opt/ros/humble/setup.bash + rosdep install -iry --from-paths src --rosdistro $ROS_DISTRO + ``` -[//]: # ( - [TensorRT](https://docs.nvidia.com/deeplearning/tensorrt/sla/index.html)) + 6. Build the workspace. + ```bash + colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release + ``` +!!! note + For detailed guidance on build issues and troubleshooting, please refer to the [Autoware Troubleshooting Guide](https://autowarefoundation.github.io/autoware-documentation/main/support/troubleshooting/#build-issues). -5. Install dependent ROS packages. - ```bash - source /opt/ros/humble/setup.bash - rosdep install -iry --from-paths src --rosdistro $ROS_DISTRO - ``` -6. Build the workspace. - ```bash - colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release - ``` + ![Build success](../image/ss2_autoware_build_result.png) + *As a result of running the `colcon build` command, all packages should be built successfully.* ## How to run ### Preparation -1. Move to project directory, where the project is build. - ```bash - cd path/to/workspace - ``` -2. Source the workspace setup script - ```bash - source install/setup.bash - ``` + + 1. Move to the workspace directory. + ```bash + cd path/to/workspace + ``` + + 2. Source the workspace setup script. + ```bash + source install/setup.bash + ``` ### Run the examples #### scenario_test_runner ```bash ros2 launch scenario_test_runner scenario_test_runner.launch.py \ - architecture_type:=awf/universe \ + architecture_type:=awf/universe/20230906 \ record:=false \ scenario:='$(find-pkg-share scenario_test_runner)/scenario/sample.yaml' \ sensor_model:=sample_sensor_kit \ vehicle_model:=sample_vehicle ``` + ![Launching scenario_test_runner](../image/scenario_test_runner_launch.gif) + *The process of launching the `scenario_test_runner` node.* + + ![Launching scenario_test_runner](../image/scenario_test_runner_result.png) + *The expected outcome of running the `scenario_test_runner` node.* #### random_test_runner ```bash ros2 launch random_test_runner random_test.launch.py \ - architecture_type:=awf/universe \ + architecture_type:=awf/universe/20230906 \ sensor_model:=sample_sensor_kit \ - vehicle_model:=sample_vehicle + vehicle_model:=sample_vehicle timeout:=120.0 ``` - - To modify parameters of random testing, please check documentation of [random_test_runner](random_test_runner/Usage.md) +!!! note + To modify parameters of random testing and to obtain more details about the test results and the **result.junit.xml** file (which is saved by default in the **/tmp** directory), please refer to the **random_test_runner** [documentation](random_test_runner/Usage.md). + + ![Launching random_test_runner](../image/random_test_runner_launch.gif) + *The process of launching the `random_test_runner` node.* + ![Result of random_test_runner](../image/random_test_runner_result.png) + *The expected outcome of running the `random_test_runner` node.* + #### cpp scenario demo - This demo does not use Autoware. the ego vehicle is interpreted as an NPC. ```bash ros2 launch cpp_mock_scenarios mock_test.launch.py \ scenario:=traffic_simulation_demo \ - launch_rviz:=true timeout:=60.0 + launch_rviz:=true timeout:=120.0 ``` + ![Launching cpp_mock_scenarios](../image/cpp_scenario_launch.gif) + *The process of launching the `cpp_mock_scenarios` node.* - ![Cpp Scenario Demo](../image/simple_demo.png "simple demo") - -[//]: # (### Troubleshooting) + ![Result of cpp_mock_scenarios](../image/cpp_scenario_result.png) + *The expected outcome of running the `cpp_mock_scenarios` node.* diff --git a/docs/user_guide/random_test_runner/Usage.md b/docs/user_guide/random_test_runner/Usage.md index 53d1ab004e8..d98db38a183 100644 --- a/docs/user_guide/random_test_runner/Usage.md +++ b/docs/user_guide/random_test_runner/Usage.md @@ -73,7 +73,7 @@ and sensor model is used in the simulation | Parameter name | Default value | Description | |---------------------|-------------------------------|---------------------------------------------------------------------------| -| `architecture_type` | `"awf/universe"` | Autoware architecture type. Currently the only supported value is: `awf/universe`. | +| `architecture_type` | `"awf/universe/20230906"` | Autoware architecture type. Currently supported values are: `awf/universe/20230906` and `awf/universe/20240605`. | | `sensor_model` | `"sample_sensor_kit"` | Ego sensor model. | | `vehicle_model` | `"sample_vehicle"` | Ego vehicle model. | diff --git a/docs/user_guide/scenario_test_runner/.pages b/docs/user_guide/scenario_test_runner/.pages index a4999386dbe..ea72fd1cbe4 100644 --- a/docs/user_guide/scenario_test_runner/.pages +++ b/docs/user_guide/scenario_test_runner/.pages @@ -2,4 +2,5 @@ nav: - Overview: ScenarioTestRunner.md - ScenarioFormatConversion.md - HowToWriteWorkflowFile.md + - RealtimeFactor.md - Tips.md diff --git a/docs/user_guide/scenario_test_runner/RealtimeFactor.md b/docs/user_guide/scenario_test_runner/RealtimeFactor.md new file mode 100644 index 00000000000..e8b1cbe5d66 --- /dev/null +++ b/docs/user_guide/scenario_test_runner/RealtimeFactor.md @@ -0,0 +1,78 @@ +# How to use realtime factor + +It is possible to modify the speed of simulation (the speed of time published on the /clock topic is not affected): + +- from the start of the simulation (using parameter), +- during the simulation (using the GUI slider). + +## Use parameter + + - When you run simulations on the command line, add an `global_real_time_factor` parameter with a custom value (the default is 1.0). + + ```bash + ros2 launch scenario_test_runner scenario_test_runner.launch.py \ + architecture_type:=awf/universe/20230906 \ + record:=false \ + scenario:='$(find-pkg-share scenario_test_runner)/scenario/sample.yaml' \ + sensor_model:=sample_sensor_kit \ + vehicle_model:=sample_vehicle \ + global_real_time_factor:="0.5" + ``` + + - The smaller the value you specify, the slower the simulation will progress. + +## Use slider on run time + +- When the simulation is started you can add the `RViz` panel by clicking `Panels -> Add new panel` in the top left corner of RViz. + +- Then in the pop up window please select `RealTimeFactorSliderPanel` and double click on it. + +![Panel](../../image/realtime_factor/panel.png) + +- Slider controlling the speed of simulation time should be visible on the left side of the screen. + +![Slider](../../image/realtime_factor/slider.png) + +- The process of adding the panel is also visible in the video: + + + + +## Configure `use_sim_time` parameter + +Parameter `use_sim_time` of `openscenario_interpreter` is **false** by default and can be modified by passing it using command line. + + + ```bash + ros2 launch scenario_test_runner scenario_test_runner.launch.py \ + architecture_type:=awf/universe/20230906 \ + record:=false \ + scenario:='$(find-pkg-share scenario_test_runner)/scenario/sample.yaml' \ + sensor_model:=sample_sensor_kit \ + vehicle_model:=sample_vehicle \ + global_real_time_factor:="0.5" \ + use_sim_time:=true + ``` + +However, this impacts the time published on the `/clock` topic and the time used by `Autoware`. +Details are shown in the table below: + +| use_sim_time launch parameter | /clock time published by scenario_simulator_v2 | AWF Autoware Time | +| ----------------------------- | ---------------------------------------------- | ---------------------- | +| false (default) | walltime | walltime from /clock | +| true | simulation | simulation from /clock | + +Below are also some bullet points explaining the impact of the `use_sim_time` parameter on `scenario_simulator_v2` and `Autoware`: + + - **`use_sim_time:=True` passed using command line** + - Both Autoware and scenario_simulator_v2 are launched with `use_sim_time=true`. + - Time published on `/clock` is the **simulation time** (starting from 0). + - Time published on `/clock` **can be** controlled by RViz plugin. + - Simulation time **can be** controlled by RViz plugin. + - **`use_sim_time:=False` passed using command line (default value)** + - Both Autoware and scenario_simulator_v2 are launched with `use_sim_time=false`. + - Time published on `/clock` is the **walltime**. + - Time published on `/clock` **cannot be** controlled by RViz plugin. + - Simulation time **can be** controlled by RViz plugin. diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 9b7ec240312..10c6e09f74b 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,284 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge pull request `#1369 `_ from tier4/fix/acquire-position-action + Fix/acquire position action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Update the clear route service call to be attempted multiple times +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Remove member function `TransitionAssertion::makeTransitionError` +* Update to check `initialize_duration` only if it has never been engaged +* Cleanup struct `TransitionAssertion` +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ +* Merge pull request `#1362 `_ from tier4/feature/ros2-parameter-forwarding +* Add feature to forward parameters prefixed with `autoware.` to Autoware +* Contributors: Kotaro Yoshimoto, yamacir-kit + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge pull request `#1338 `_ from tier4/fix/interpreter/user-defined-value-condition + Fix/interpreter/user defined value condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Change QoS of `/api/localization/initialization_state` to transient local +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index e9d14665928..ff9db9b0b73 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -84,6 +84,11 @@ class FieldOperatorApplicationFor auto receiveEmergencyState(const tier4_external_api_msgs::msg::Emergency & msg) -> void; + /* + NOTE: This predicate should not take the state being compared as an + argument or template parameter. Otherwise, code using this class would + need to have knowledge of the Autoware state type. + */ #define DEFINE_STATE_PREDICATE(NAME, VALUE) \ auto is##NAME() const noexcept \ { \ @@ -114,17 +119,17 @@ class FieldOperatorApplicationFor CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), // clang-format off - getAckermannControlCommand("/control/command/control_cmd", *this), - getAutowareState("/api/iv_msgs/autoware/state", *this), - getCooperateStatusArray("/api/external/get/rtc_status", *this, [this](const auto & v) { latest_cooperate_status_array = v; }), - getEmergencyState("/api/external/get/emergency", *this, [this](const auto & v) { receiveEmergencyState(v); }), + getAckermannControlCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), + getAutowareState("/api/iv_msgs/autoware/state", rclcpp::QoS(1), *this), + getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), + getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & v) { receiveEmergencyState(v); }), #if __has_include() - getLocalizationState("/api/localization/initialization_state", *this), + getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), #endif - getMrmState("/api/fail_safe/mrm_state", *this, [this](const auto & v) { receiveMrmState(v); }), - getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this), - getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", *this), - getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", *this), + getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & v) { receiveMrmState(v); }), + getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), + getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), + getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), requestClearRoute("/api/routing/clear_route", *this), requestCooperateCommands("/api/external/set/rtc_commands", *this), requestEngage("/api/external/set/engage", *this), diff --git a/external/concealer/include/concealer/launch.hpp b/external/concealer/include/concealer/launch.hpp index 99ba2556654..54f1459f1b3 100644 --- a/external/concealer/include/concealer/launch.hpp +++ b/external/concealer/include/concealer/launch.hpp @@ -33,8 +33,9 @@ namespace concealer { -template -auto ros2_launch(const std::string & package, const std::string & file, Ts &&... xs) +template +auto ros2_launch( + const std::string & package, const std::string & file, const Parameters & parameters) { #ifdef CONCEALER_ISOLATE_STANDARD_OUTPUT const std::string log_filename = "/tmp/scenario_test_runner/autoware-output.txt"; @@ -44,17 +45,23 @@ auto ros2_launch(const std::string & package, const std::string & file, Ts &&... ::close(fd); #endif - const auto process_id = fork(); + const auto argv = [&]() { + auto argv = std::vector(); - const std::vector argv{ - "python3", - boost::algorithm::replace_all_copy(dollar("which ros2"), "\n", ""), - "launch", // NOTE: The command 'ros2' is a Python script. - package, - file, - std::forward(xs)...}; + argv.push_back("python3"); + argv.push_back(boost::algorithm::replace_all_copy(dollar("which ros2"), "\n", "")); + argv.push_back("launch"); + argv.push_back(package); + argv.push_back(file); - if (process_id < 0) { + for (const auto & parameter : parameters) { + argv.push_back(parameter); + } + + return argv; + }(); + + if (const auto process_id = fork(); process_id < 0) { throw std::system_error(errno, std::system_category()); } else if (process_id == 0 and execute(argv) < 0) { std::cout << std::system_error(errno, std::system_category()).what() << std::endl; diff --git a/external/concealer/include/concealer/subscriber_wrapper.hpp b/external/concealer/include/concealer/subscriber_wrapper.hpp index e8869fb86ce..8247a872e3a 100644 --- a/external/concealer/include/concealer/subscriber_wrapper.hpp +++ b/external/concealer/include/concealer/subscriber_wrapper.hpp @@ -41,10 +41,12 @@ class SubscriberWrapper template SubscriberWrapper( - const std::string & topic, NodeInterface & autoware_interface, + const std::string & topic, const rclcpp::QoS & quality_of_service, + NodeInterface & autoware_interface, const std::function & callback = {}) : subscription(autoware_interface.template create_subscription( - topic, 1, [this, callback](const typename MessageType::ConstSharedPtr message) { + topic, quality_of_service, + [this, callback](const typename MessageType::ConstSharedPtr message) { if constexpr (thread_safety == ThreadSafety::safe) { std::atomic_store(¤t_value, message); if (current_value and callback) { diff --git a/external/concealer/include/concealer/transition_assertion.hpp b/external/concealer/include/concealer/transition_assertion.hpp index 55724ba70b8..7850899362e 100644 --- a/external/concealer/include/concealer/transition_assertion.hpp +++ b/external/concealer/include/concealer/transition_assertion.hpp @@ -15,7 +15,6 @@ #ifndef CONCEALER__TRANSITION_ASSERTION_HPP_ #define CONCEALER__TRANSITION_ASSERTION_HPP_ -#include #include #include #include @@ -23,103 +22,54 @@ namespace concealer { -template -auto getParameter(const std::string & name, T value = {}) -{ - rclcpp::Node node{"get_parameter", "simulation"}; - - node.declare_parameter(name, value); - node.get_parameter(name, value); - - return value; -} - template struct TransitionAssertion { - using clock_type = std::chrono::steady_clock; - using rate_type = rclcpp::GenericRate; + const std::chrono::steady_clock::time_point start; - const clock_type::time_point start; const std::chrono::seconds initialize_duration; - explicit TransitionAssertion() - : start(clock_type::now()), - initialize_duration(std::chrono::seconds(getParameter("initialize_duration"))) - { - } + bool have_never_been_engaged = true; - auto makeTransitionError(const std::string & expected) const + explicit TransitionAssertion() + : start(std::chrono::steady_clock::now()), initialize_duration([]() { + auto node = rclcpp::Node("get_parameter", "simulation"); + node.declare_parameter("initialize_duration", 0); + return node.get_parameter("initialize_duration").as_int(); + }()) { - const auto current_state = asAutoware().getAutowareStateName(); - using namespace std::chrono; - return common::AutowareError( - "Simulator waited for the Autoware state to transition to ", expected, - ", but time is up. The current Autoware state is ", - (current_state.empty() ? "NOT PUBLISHED YET" : current_state), "."); } - auto asAutoware() -> Autoware & { return static_cast(*this); } - auto asAutoware() const -> const Autoware & { return static_cast(*this); } - -#define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(STATE) \ - template \ - void waitForAutowareStateToBe##STATE( \ - Thunk && thunk = [] {}, std::chrono::seconds interval = std::chrono::seconds(1)) \ - { \ - using std::chrono::duration_cast; \ - using std::chrono::seconds; \ - rate_type rate(interval); \ - for (thunk(); not asAutoware().isStopRequested() and not asAutoware().is##STATE(); \ - rate.sleep()) { \ - auto now = clock_type::now(); \ - if (start + initialize_duration <= now) { \ - throw makeTransitionError(#STATE); \ - } else { \ - RCLCPP_INFO_STREAM( \ - asAutoware().get_logger(), \ - "Simulator waiting for Autoware state to be " #STATE " (remain: " \ - << duration_cast(start + initialize_duration - now).count() << ")."); \ - thunk(); \ - } \ - } \ - RCLCPP_INFO_STREAM( \ - asAutoware().get_logger(), \ - "Autoware is " << asAutoware().getAutowareStateName() << " now."); \ - } \ - static_assert(true, "") - -#define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \ - template \ - void waitForAutowareStateToBe##STATE( \ - Thunk && thunk = [] {}, std::chrono::seconds interval = std::chrono::seconds(1)) \ - { \ - using std::chrono::duration_cast; \ - using std::chrono::seconds; \ - rate_type rate(interval); \ - for (thunk(); not asAutoware().isStopRequested() and not asAutoware().is##STATE(); \ - rate.sleep()) { \ - auto now = clock_type::now(); \ - RCLCPP_INFO_STREAM( \ - asAutoware().get_logger(), \ - "Simulator waiting for Autoware state to be " #STATE " (remain: " \ - << duration_cast(start + initialize_duration - now).count() << ")."); \ - thunk(); \ - } \ - RCLCPP_INFO_STREAM( \ - asAutoware().get_logger(), \ - "Autoware is " << asAutoware().getAutowareStateName() << " now."); \ - } \ - static_assert(true, "") - - // XXX: The time limit must be ignored in waitForAutowareStateToBeDriving() because the current - // implementation attempts to transition to Driving state after initialize_duration seconds have - // elapsed. - - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(Initializing); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(WaitingForRoute); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(Planning); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE_WITH_INITIALIZE_TIME_LIMIT(WaitingForEngage); +#define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \ + template \ + auto waitForAutowareStateToBe##STATE( \ + Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \ + { \ + for (thunk(); not static_cast(*this).isStopRequested() and \ + not static_cast(*this).is##STATE(); \ + rclcpp::GenericRate(interval).sleep()) { \ + if ( \ + have_never_been_engaged and \ + start + initialize_duration <= std::chrono::steady_clock::now()) { \ + const auto state = static_cast(*this).getAutowareStateName(); \ + throw common::AutowareError( \ + "Simulator waited for the Autoware state to transition to " #STATE \ + ", but time is up. The current Autoware state is ", \ + (state.empty() ? "not published yet" : state)); \ + } else { \ + thunk(); \ + } \ + } \ + if constexpr (std::string_view(#STATE) == std::string_view("Driving")) { \ + have_never_been_engaged = false; \ + } \ + } \ + static_assert(true) + + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Initializing); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForRoute); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Planning); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForEngage); DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Driving); DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(ArrivedGoal); DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Finalizing); diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 0470dd272bb..18794722926 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 3.3.0 + 4.3.7 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 6016eb0d322..dbecc8b51d9 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -17,11 +17,12 @@ namespace concealer { AutowareUniverse::AutowareUniverse() -: getAckermannControlCommand("/control/command/control_cmd", *this), - getGearCommandImpl("/control/command/gear_cmd", *this), - getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", *this), +: getAckermannControlCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), + getGearCommandImpl("/control/command/gear_cmd", rclcpp::QoS(1), *this), + getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), getPathWithLaneId( - "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", *this), + "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), + *this), setAcceleration("/localization/acceleration", *this), setOdometry("/localization/kinematic_state", *this), setSteeringReport("/vehicle/status/steering_status", *this), diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index b86d4b3a657..83280ab9644 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -350,7 +350,12 @@ auto FieldOperatorApplicationFor::plan( auto FieldOperatorApplicationFor::clearRoute() -> void { task_queue.delay([this] { - requestClearRoute(std::make_shared()); + /* + Since this service tends to be available long after the launch of + Autoware, set the attempts_count to a high value. There is no technical + basis for the number 30. + */ + requestClearRoute(std::make_shared(), 30); }); } diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 4501671a247..9f526c510c2 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,271 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 1b54a660046..ec27879d434 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 3.3.0 + 4.3.7 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 46d89ea6249..6b1db05c7e5 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index ce4b1ededfc..12b92cb234c 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 3.3.0 + 4.3.7 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst index 22282cbb198..1a64ed9372a 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,249 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index acbf5dd783f..fb0f45b125b 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 3.3.0 + 4.3.7 map package for simple cross Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index bc906333fd7..34ad3894a63 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,297 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ +* Merge pull request `#1379 `_ from tier4/fix/hard-coded-update-rate + fix(mock): hard-coded update rate +* fix(mock): hard-coded update rate +* Contributors: Masaya Kataoka, satoshi-ota + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ +* Merge pull request `#1373 `_ from tier4/fix/colcon_build_error_furthermore + fix: install add_cpp_mock_scenario_test.cmake first, or colcon build won't pass +* fix: install add_cpp_mock_scenario_test.cmake first, or build won't pass +* Contributors: Masaya Kataoka, XiaoyuWang0601 + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ +* Merge pull request `#1368 `_ from tier4/fix/mock-test-launch-test + fix: mock test launch +* fix: global frame rate 30.0 -> 20.0 +* fix: set default rviz config +* fix: missing param +* fix: use global timeout +* fix: make it possible to change hard-coded parameters +* fix: load necessary parameters +* Contributors: Masaya Kataoka, satoshi-ota + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(traffic_simulator): use getCanonicalizedStatus, remove getStatus +* feat(cpp_mock_scenarios): add isPedestrain and isVehicle - use it +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(cpp_mack_utils): adapt define_traffic_source scenarios to getEntity() +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ +* Merge pull request `#1339 `_ from tier4/fix/ament_auto_package + fix CONFIG_EXTRAS argument of ament_auto_package macro +* fix ament_auto_package macro +* Contributors: Masaya Kataoka + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/mock/cpp_mock_scenarios/CMakeLists.txt b/mock/cpp_mock_scenarios/CMakeLists.txt index 308df5bf84a..669465e6482 100644 --- a/mock/cpp_mock_scenarios/CMakeLists.txt +++ b/mock/cpp_mock_scenarios/CMakeLists.txt @@ -64,6 +64,9 @@ endif() install( DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}) +install( + FILES cmake/add_cpp_mock_scenario_test.cmake + DESTINATION share/${PROJECT_NAME}/cmake) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -73,4 +76,4 @@ if(BUILD_TESTING) # add_cpp_mock_scenario_test(${PROJECT_NAME} "traffic_simulation_demo" "70") endif() -ament_auto_package(CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake") +ament_auto_package(CONFIG_EXTRAS "${PROJECT_NAME}_ament_cmake-extras.cmake") diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index aa7df2b5dea..4d1141c8fc7 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -68,7 +68,7 @@ class CppScenarioNode : public rclcpp::Node virtual void onUpdate() = 0; virtual void onInitialize() = 0; rclcpp::TimerBase::SharedPtr update_timer_; - double timeout_; + int timeout_; auto configure( const std::string & map_path, const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose) -> traffic_simulator::Configuration diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index d976cf696ac..0289e874095 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -30,7 +30,7 @@ from launch.events import Shutdown from launch.event_handlers import OnProcessExit, OnProcessIO -from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction +from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction, OpaqueFunction from launch.actions.declare_launch_argument import DeclareLaunchArgument from launch.substitutions.launch_configuration import LaunchConfiguration @@ -40,7 +40,6 @@ from pathlib import Path - class Color: BLACK = "\033[30m" RED = "\033[31m" @@ -70,113 +69,190 @@ def on_stdout_output(event: launch.Event) -> None: if lines[0] == "cpp_scenario:success": print(Color.GREEN + "Scenario Succeed" + Color.END) +def architecture_types(): + return ["awf/universe", "awf/universe/20230906"] -def generate_launch_description(): - timeout = LaunchConfiguration("timeout", default=10.0) - scenario = LaunchConfiguration("scenario", default="") - scenario_package = LaunchConfiguration("package", default="cpp_mock_scenarios") - junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") - launch_rviz = LaunchConfiguration("launch_rviz", default=False) - vehicle_model = LaunchConfiguration("vehicle_model", default="sample_vehicle") - sensor_model = LaunchConfiguration("sensor_model", default="sample_sensor_kit") - consider_pose_by_road_slope = LaunchConfiguration("consider_pose_by_road_slope", default=True) - scenario_node = Node( - package=scenario_package, - executable=scenario, - name="scenario_node", - output="screen", - arguments=[("__log_level:=info")], - parameters=[ - { - "junit_path": junit_path, - "timeout": timeout, - "architecture_type": "awf/universe", - "autoware_launch_file": "planning_simulator.launch.xml", - "autoware_launch_package": "autoware_launch", - "launch_autoware": True, - "vehicle_model": vehicle_model, - "sensor_model": sensor_model, - "initialize_duration": 900, - "consider_pose_by_road_slope": consider_pose_by_road_slope - } - ], - ) + +def default_autoware_launch_package_of(architecture_type): + if architecture_type not in architecture_types(): + raise KeyError( + f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}." + ) + return { + "awf/universe": "autoware_launch", + "awf/universe/20230906": "autoware_launch", + }[architecture_type] + + +def default_autoware_launch_file_of(architecture_type): + if architecture_type not in architecture_types(): + raise KeyError( + f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}." + ) + return { + "awf/universe": "planning_simulator.launch.xml", + "awf/universe/20230906": "planning_simulator.launch.xml", + }[architecture_type] + +def default_rviz_config_file(): + return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" + +def launch_setup(context, *args, **kwargs): + # fmt: off + architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20230906") + autoware_launch_file = LaunchConfiguration("autoware_launch_file", default=default_autoware_launch_file_of(architecture_type.perform(context))) + autoware_launch_package = LaunchConfiguration("autoware_launch_package", default=default_autoware_launch_package_of(architecture_type.perform(context))) + consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope", default=False) + consider_pose_by_road_slope = LaunchConfiguration("consider_pose_by_road_slope", default=True) + global_frame_rate = LaunchConfiguration("global_frame_rate", default=20.0) + global_real_time_factor = LaunchConfiguration("global_real_time_factor", default=1.0) + global_timeout = LaunchConfiguration("global_timeout", default=180) + initialize_duration = LaunchConfiguration("initialize_duration", default=300) + launch_autoware = LaunchConfiguration("launch_autoware", default=True) + launch_rviz = LaunchConfiguration("launch_rviz", default=False) + launch_simple_sensor_simulator = LaunchConfiguration("launch_simple_sensor_simulator", default=True) + output_directory = LaunchConfiguration("output_directory", default=Path("/tmp")) + port = LaunchConfiguration("port", default=5555) + publish_empty_context = LaunchConfiguration("publish_empty_context", default=False) + record = LaunchConfiguration("record", default=False) + rviz_config = LaunchConfiguration("rviz_config", default=default_rviz_config_file()) + scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) + sensor_model = LaunchConfiguration("sensor_model", default="") + sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) + use_sim_time = LaunchConfiguration("use_sim_time", default=False) + vehicle_model = LaunchConfiguration("vehicle_model", default="") + scenario_package = LaunchConfiguration("package", default="cpp_mock_scenarios") + junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml") + # fmt: on + + print(f"architecture_type := {architecture_type.perform(context)}") + print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") + print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") + print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}") + print(f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}") + print(f"global_frame_rate := {global_frame_rate.perform(context)}") + print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") + print(f"global_timeout := {global_timeout.perform(context)}") + print(f"initialize_duration := {initialize_duration.perform(context)}") + print(f"launch_autoware := {launch_autoware.perform(context)}") + print(f"launch_rviz := {launch_rviz.perform(context)}") + print(f"output_directory := {output_directory.perform(context)}") + print(f"port := {port.perform(context)}") + print(f"publish_empty_context := {publish_empty_context.perform(context)}") + print(f"record := {record.perform(context)}") + print(f"rviz_config := {rviz_config.perform(context)}") + print(f"scenario := {scenario.perform(context)}") + print(f"sensor_model := {sensor_model.perform(context)}") + print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") + print(f"use_sim_time := {use_sim_time.perform(context)}") + print(f"vehicle_model := {vehicle_model.perform(context)}") + print(f"scenario_package := {scenario_package.perform(context)}") + print(f"junit_path := {junit_path.perform(context)}") + + def make_parameters(): + parameters = [ + {"architecture_type": architecture_type}, + {"autoware_launch_file": autoware_launch_file}, + {"autoware_launch_package": autoware_launch_package}, + {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope}, + {"consider_pose_by_road_slope": consider_pose_by_road_slope}, + {"initialize_duration": initialize_duration}, + {"launch_autoware": launch_autoware}, + {"port": port}, + {"publish_empty_context" : publish_empty_context}, + {"record": record}, + {"rviz_config": rviz_config}, + {"sensor_model": sensor_model}, + {"sigterm_timeout": sigterm_timeout}, + {"vehicle_model": vehicle_model}, + {"global_real_time_factor": global_real_time_factor}, + {"global_frame_rate": global_frame_rate}, + {"global_timeout": global_timeout}, + {"junit_path": junit_path}, + ] + parameters += make_vehicle_parameters() + return parameters + + def make_vehicle_parameters(): + parameters = [] + + def description(): + return get_package_share_directory( + vehicle_model.perform(context) + "_description" + ) + + if vehicle_model.perform(context): + parameters.append(description() + "/config/vehicle_info.param.yaml") + parameters.append(description() + "/config/simulator_model.param.yaml") + return parameters + + cpp_scenario_node = Node( + package=scenario_package, + executable=scenario, + name="scenario_node", + output="screen", + arguments=[("__log_level:=info")], + parameters=make_parameters() + [{"use_sim_time": use_sim_time}], + ) io_handler = OnProcessIO( - target_action=scenario_node, + target_action=cpp_scenario_node, on_stderr=on_stderr_output, on_stdout=on_stdout_output, ) shutdown_handler = OnProcessExit( - target_action=scenario_node, on_exit=[EmitEvent(event=Shutdown())] + target_action=cpp_scenario_node, on_exit=[EmitEvent(event=Shutdown())] ) - description = LaunchDescription( - [ - DeclareLaunchArgument( - "scenario", default_value=scenario, description="Name of the scenario." - ), - DeclareLaunchArgument( - "package", - default_value=scenario_package, - description="Name of package your scenario exists", - ), - DeclareLaunchArgument( - "timeout", default_value=timeout, description="Timeout in seconds." - ), - DeclareLaunchArgument( - "junit_path", - default_value=junit_path, - description="Path of the junit output.", - ), - DeclareLaunchArgument( - "launch_rviz", - default_value=launch_rviz, - description="If true, launch with rviz.", - ), - DeclareLaunchArgument( - "vehicle_model", - default_value=vehicle_model, - description="Vehicle model of the Autoware", - ), - DeclareLaunchArgument( - "sensor_model", - default_value=sensor_model, - description="Sensor model of the Autoware", - ), - DeclareLaunchArgument( - "consider_pose_by_road_slope", - default_value=consider_pose_by_road_slope , - description="Consideration of lanelet slope in matching position", - ), - scenario_node, - RegisterEventHandler(event_handler=io_handler), - RegisterEventHandler(event_handler=shutdown_handler), - Node( - package="simple_sensor_simulator", - executable="simple_sensor_simulator_node", - name="simple_sensor_simulator_node", - output="log", - arguments=[("__log_level:=warn")], - ), - Node( - package="traffic_simulator", - executable="visualization_node", - name="visualizer", - output="screen", - ), - Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output={"stderr": "log", "stdout": "log"}, - condition=IfCondition(launch_rviz), - arguments=[ - "-d", - str( - Path(get_package_share_directory("cpp_mock_scenarios")) - / "rviz/mock_test.rviz" - ), - ], - ), - ] - ) - return description + + return [ + # fmt: off + DeclareLaunchArgument("architecture_type", default_value=architecture_type ), + DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file ), + DeclareLaunchArgument("autoware_launch_package", default_value=autoware_launch_package ), + DeclareLaunchArgument("consider_acceleration_by_road_slope", default_value=consider_acceleration_by_road_slope), + DeclareLaunchArgument("consider_pose_by_road_slope", default_value=consider_pose_by_road_slope ), + DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate ), + DeclareLaunchArgument("global_real_time_factor", default_value=global_real_time_factor ), + DeclareLaunchArgument("global_timeout", default_value=global_timeout ), + DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), + DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), + DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ), + DeclareLaunchArgument("output_directory", default_value=output_directory ), + DeclareLaunchArgument("rviz_config", default_value=rviz_config ), + DeclareLaunchArgument("scenario", default_value=scenario ), + DeclareLaunchArgument("sensor_model", default_value=sensor_model ), + DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), + DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), + DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), + DeclareLaunchArgument("scenario_package", default_value=scenario_package ), + DeclareLaunchArgument("junit_path", default_value=junit_path ), + # fmt: on + cpp_scenario_node, + Node( + package="simple_sensor_simulator", + executable="simple_sensor_simulator_node", + namespace="simulation", + output="screen", + parameters=make_parameters() + [{"use_sim_time": use_sim_time}], + condition=IfCondition(launch_simple_sensor_simulator), + ), + Node( + package="traffic_simulator", + executable="visualization_node", + namespace="simulation", + name="visualizer", + output="screen", + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output={"stderr": "log", "stdout": "log"}, + condition=IfCondition(launch_rviz), + arguments=["-d", str(default_rviz_config_file())], + ), + RegisterEventHandler(event_handler=io_handler), + RegisterEventHandler(event_handler=shutdown_handler), + ] + +def generate_launch_description(): + return LaunchDescription([OpaqueFunction(function=launch_setup)]) diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 255412c1756..cd32247f35e 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 3.3.0 + 4.3.7 C++ mock scenarios masaya Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index cc875773285..35508fae892 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -22,14 +22,17 @@ CppScenarioNode::CppScenarioNode( const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose, const rclcpp::NodeOptions & option) : Node(node_name, option), - api_(this, configure(map_path, lanelet2_map_file, scenario_filename, verbose), 1.0, 20), + api_( + this, configure(map_path, lanelet2_map_file, scenario_filename, verbose), + declare_parameter("global_real_time_factor", 1.0), + declare_parameter("global_frame_rate", 20.0)), scenario_filename_(scenario_filename), exception_expect_(false) { declare_parameter("junit_path", "/tmp"); get_parameter("junit_path", junit_path_); - declare_parameter("timeout", 10.0); - get_parameter("timeout", timeout_); + declare_parameter("global_timeout", 10.0); + get_parameter("global_timeout", timeout_); traffic_simulator::lanelet_pose::CanonicalizedLaneletPose::setConsiderPoseByRoadSlope([&]() { if (not has_parameter("consider_pose_by_road_slope")) { @@ -61,8 +64,9 @@ void CppScenarioNode::start() { onInitialize(); api_.startNpcLogic(); - using namespace std::chrono_literals; - update_timer_ = this->create_wall_timer(50ms, std::bind(&CppScenarioNode::update, this)); + const auto rate = + std::chrono::duration(1.0 / get_parameter("global_frame_rate").as_double()); + update_timer_ = this->create_wall_timer(rate, std::bind(&CppScenarioNode::update, this)); } void CppScenarioNode::stop(Result result, const std::string & description) diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 39a4d45eb67..44e02447922 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 5fa44633a10..630abbefd8e 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 3.3.0 + 4.3.7 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index c21bae1b89e..72fa88ce15d 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,337 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge pull request `#1369 `_ from tier4/fix/acquire-position-action + Fix/acquire position action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Update the scenario to drive laps around the closed route +* Update `AcquirePositionAction` to request route clearing before applying +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge pull request `#1388 `_ from tier4/fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* fix: use base_link for default imu frame_id +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge pull request `#1338 `_ from tier4/fix/interpreter/user-defined-value-condition + Fix/interpreter/user defined value condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Update `MagicSubscription`'s QoS to best effort +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Cleanup struct `MagicSubscription` +* Update MagicSubscription to share resources between instances +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge pull request `#1316 `_ from tier4/relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* fix: treat added entity only in RelativeClearanceCondition +* refactor: delete RelativeClearanceCondition::getRelativeLanePosition +* refactor: use exception when errors are occurred in SimulatorCore::evaluateLateralRelativeLanes +* refactor: use std::optional for optional attribute in RelativeLaneRange +* feat: support EntitySelection in RelativeClearanceCondition +* refactor: use boost::math::constants::half_pi instead of 0.5 * boost::math::constants::pi +* fix: update target entities of RelativeClearanceCondition in every frame +* refactor: use boost::math::constants::pi() instead of M_PI +* fix: implement Integer::min/max instead of Integer::infinity +* Merge branch 'master' into relative-clearance-condition +* feat: improve description output of RelativeClearanceCondition +* Merge branch 'master' into relative-clearance-condition +* refactor: format comment-outs +* Merge branch 'master' into relative-clearance-condition +* refactor: clean up includes in relative_lane_range.hpp +* Fix RelativeLaneRange to use default values in the specification +* Add Integer::infinity function +* apply linter +* Fix copy bugs in RelativeClearanceCondition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Implement switching by relative heading of triggering_entity in RelativeClearanceCondition +* fix condition logic of RelativeClearanceCondition +* Correct initialization of RelativeClearanceCondition::entity_refs along the standard +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Revert "Move entity existence check into `distance` from speceialized `distance`" + This reverts commit 727d57dc93f29badb41661fcb8543c9ce7840392. +* Revert "Update `RelativeDistanceCondition::distance` to static member function" + This reverts commit 86f489f0 +* Add temporary implementation of RelativeClearanceCondition::evaluate function +* Implement RelativeClearanceCondition::getRelativeLanePosition function +* Implement SimulatorCore::evaluateLateralRelativeLanes function +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* refactor: import RelativeDistanceCondition updates from feature/time-to-collision-condition branch + Co-authored-by: yamacir-kit +* Merge branch 'master' into relative-clearance-condition +* Update `RelativeDistanceCondition::distance` to static member function +* Move entity existence check into `distance` from speceialized `distance` +* fix: replace freespace with freeSpace in RelativeClearanceCondition +* chore: update OpenSCENARIO version of EntityCondition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* feat(openscenario_interpreter): add RelativeClearanceCondition(empty implementation) +* feat(openscenario_interpreter): add RelativeLaneRange +* Revert "fix(RelativeDistanceCondition): Fixed a bug where RelativeDistance showed negative values" + This reverts commit 84c8b0c101b8e680ad6029d8702387e5495e2646. +* fix(RelativeDistanceCondition): Fixed a bug where RelativeDistance showed negative values +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki, yamacir-kit + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge branch 'master' into doc/longitudinal-control +* Merge pull request `#1321 `_ from tier4/feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* feat: Enhance IMU sensor configuration and initialization + - Added frame_id to ImuSensorConfiguration + - Separated noise standard deviations for orientation, twist, and acceleration + - Updated ImuSensorBase and ImuSensor classes for new noise distributions +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* feat(simple_sensor_simulator, imu): add gravity vector, tidy up +* feat(simulator_core, api, zmq): add attachImuSensor, add update imu sensors +* Contributors: Dawid Moszynski, Koki Suzuki, Kotaro Yoshimoto, Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ +* Merge pull request `#1325 `_ from tier4/feature/interpreter/lidar-configuration + Feature/interpreter/lidar configuration +* Add a test scenario for `ObjectController`'s pseudo LiDAR property +* Update ControllerAction to support some new properties related to LiDAR +* Contributors: Masaya Kataoka, yamacir-kit + 3.3.0 (2024-07-23) ------------------ * Merge pull request `#1059 `_ from tier4/feature/interpreter/entity_selection diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp index 06568802cbe..945e1a346a3 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/reader/attribute.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -164,6 +165,16 @@ auto readAttribute(const std::string & name, const Node & node, const Scope & sc return value; } } + +template +auto readAttribute(const std::string & name, const Node & node, const Scope & scope, std::nullopt_t) +{ + if (node.attribute(name.c_str())) { + return std::make_optional(readAttribute(name, node, scope)); + } else { + return std::optional(); + } +} } // namespace reader } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 3d65a40ac4a..0bfae4f7fb7 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -256,6 +256,28 @@ class SimulatorCore } return traffic_simulator::pose::quietNaNPose(); } + + static auto evaluateLateralRelativeLanes( + const std::string & from_entity_name, const std::string & to_entity_name, + const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> int + { + if (const auto from_entity = core->getEntity(from_entity_name)) { + if (const auto to_entity = core->getEntity(to_entity_name)) { + const bool allow_lane_change = + (routing_algorithm == RoutingAlgorithm::value_type::shortest); + if ( + auto lane_changes = traffic_simulator::distance::countLaneChanges( + from_entity->getCanonicalizedLaneletPose().value(), + to_entity->getCanonicalizedLaneletPose().value(), allow_lane_change, + core->getHdmapUtils())) { + return lane_changes.value().first - lane_changes.value().second; + } + } + } + throw common::Error( + "Failed to evaluate lateral relative lanes between ", from_entity_name, " and ", + to_entity_name); + } }; class ActionApplication // OpenSCENARIO 1.1.1 Section 3.1.5 @@ -265,6 +287,7 @@ class SimulatorCore static auto applyAcquirePositionAction(const std::string & entity_ref, Ts &&... xs) { auto entity = core->getEntity(entity_ref); + entity->requestClearRoute(); return entity->requestAcquirePosition(std::forward(xs)...); } @@ -337,8 +360,48 @@ class SimulatorCore if (controller.isAutoware()) { auto ego_entity = core->getEgoEntity(entity_ref); - core->attachLidarSensor( - entity_ref, controller.properties.template get("pointcloudPublishingDelay")); + + core->attachImuSensor(entity_ref, [&]() { + simulation_api_schema::ImuSensorConfiguration configuration; + configuration.set_entity(entity_ref); + configuration.set_frame_id("base_link"); + configuration.set_add_gravity(true); + configuration.set_use_seed(true); + configuration.set_seed(0); + configuration.set_noise_standard_deviation_orientation(0.01); + configuration.set_noise_standard_deviation_twist(0.01); + configuration.set_noise_standard_deviation_acceleration(0.01); + return configuration; + }()); + + core->attachLidarSensor([&]() { + simulation_api_schema::LidarConfiguration configuration; + + auto degree_to_radian = [](auto degree) { + return degree / 180.0 * boost::math::constants::pi(); + }; + + // clang-format off + configuration.set_architecture_type(core->getROS2Parameter("architecture_type", "awf/universe")); + configuration.set_entity(entity_ref); + configuration.set_horizontal_resolution(degree_to_radian(controller.properties.template get("pointcloudHorizontalResolution", 1.0))); + configuration.set_lidar_sensor_delay(controller.properties.template get("pointcloudPublishingDelay")); + configuration.set_scan_duration(0.1); + // clang-format on + + const auto vertical_field_of_view = degree_to_radian( + controller.properties.template get("pointcloudVerticalFieldOfView", 30.0)); + + const auto channels = + controller.properties.template get("pointcloudChannels", 16); + + for (std::size_t i = 0; i < channels; ++i) { + configuration.add_vertical_angles( + vertical_field_of_view / 2 - vertical_field_of_view / channels * i); + } + + return configuration; + }()); core->attachDetectionSensor([&]() { simulation_api_schema::DetectionSensorConfiguration configuration; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_condition.hpp index 19add645326..e1a4f23ccab 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_condition.hpp @@ -23,27 +23,38 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- EntityCondition -------------------------------------------------------- - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * - * -------------------------------------------------------------------------- */ +/* + EntityCondition (OpenSCENARIO XML 1.3) + + Defines a specific condition on an entity. + + + + + + + + + + + + + + + + + deprecated + + + + + + + + + + + */ struct EntityCondition : public ComplexType { explicit EntityCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/integer.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/integer.hpp index b074cf26ae6..4b57eb67cb8 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/integer.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/integer.hpp @@ -35,6 +35,10 @@ struct Integer explicit Integer(const std::string &); + static auto max() noexcept -> Integer; + + static auto min() noexcept -> Integer; + auto operator+=(const double &) -> Integer &; auto operator*=(const double &) -> Integer &; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_clearance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_clearance_condition.hpp new file mode 100644 index 00000000000..804c60e50e6 --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_clearance_condition.hpp @@ -0,0 +1,88 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_CLEARANCE_CONDITION_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_CLEARANCE_CONDITION_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + RelativeClearanceCondition (OpenSCENARIO XML 1.3) + + + + + + + + + + + +*/ +struct RelativeClearanceCondition : private Scope, + private SimulatorCore::ConditionEvaluation, + private SimulatorCore::NonStandardOperation +{ + /* + Longitudinal distance behind reference point of the entity to be checked along lane centerline of the current lane of the triggering entity. Orientation of entity towards lane determines backward direction. Velocity of entity is irrelevant. Unit: [m]. Range: [0..inf[. Default if omitted: 0 + */ + const Double distance_backward; + + /* + Longitudinal distance in front of reference point of the entity to be checked along lane centerline of the current lane of the triggering entity. Orientation of entity towards lane determines forward direction. Velocity of entity is irrelevant. Unit: [m]. Range: [0..inf[. Default if omitted: 0 + */ + const Double distance_forward; + + /* + If false, then entityRefs are only considered to be on the lane if their reference point is within the checked area; otherwise the whole bounding box is considered. + */ + const Boolean free_space; + + /* + If true, then also lanes in the opposite direction are considered; otherwise only lanes in the same direction are considered. + */ + const Boolean opposite_lanes; + + /* + The lanes to be checked to left and right of the triggering entity (positive to the y-axis). If omitted: all lanes are checked. + */ + const std::list relative_lane_range; + + /* + Constraint to check only specific entities. If it is not used then all entities are considered. + */ + const std::list entity_refs; + + const TriggeringEntities triggering_entities; + + explicit RelativeClearanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); + + auto description() const -> String; + + auto evaluate() -> Object; +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_CLEARANCE_CONDITION_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_lane_range.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_lane_range.hpp new file mode 100644 index 00000000000..f20d562173e --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_lane_range.hpp @@ -0,0 +1,54 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_LANE_RANGE_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_LANE_RANGE_HPP_ + +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* + RelativeLaneRange (OpenSCENARIO XML 1.3) + + + + + +*/ +struct RelativeLaneRange +{ + /* + The lower limit of the range. Range: [-inf, inf[. Default if omitted: -inf + */ + const std::optional from; + + /* + The upper limit of the range. Range: ]-inf, inf]. Default if omitted: +inf + */ + const std::optional to; + + explicit RelativeLaneRange(const pugi::xml_node &, Scope &); + + auto evaluate(Integer::value_type value) const -> bool; +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_LANE_RANGE_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp index 585d6d9e273..7ca1707fd9e 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/user_defined_value_condition.hpp @@ -45,8 +45,6 @@ class UserDefinedValueCondition : private SimulatorCore::NonStandardOperation std::function evaluate_value; - static uint64_t magic_subscription_counter; - public: const String name; diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index ffdbdffdd2f..0791ccfdf02 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 3.3.0 + 4.3.7 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp index 9577b8f9453..df89c5ba0fe 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_condition.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -32,19 +33,22 @@ EntityCondition::EntityCondition( // clang-format off : ComplexType( choice(node, - std::make_pair( "EndOfRoadCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), - std::make_pair( "CollisionCondition", [&](const auto & node) { return make< CollisionCondition>(node, scope, triggering_entities); }), - std::make_pair( "OffroadCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), - std::make_pair( "TimeHeadwayCondition", [&](const auto & node) { return make< TimeHeadwayCondition>(node, scope, triggering_entities); }), - std::make_pair( "TimeToCollisionCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), - std::make_pair( "AccelerationCondition", [&](const auto & node) { return make< AccelerationCondition>(node, scope, triggering_entities); }), - std::make_pair( "StandStillCondition", [&](const auto & node) { return make< StandStillCondition>(node, scope, triggering_entities); }), - std::make_pair( "SpeedCondition", [&](const auto & node) { return make< SpeedCondition>(node, scope, triggering_entities); }), - std::make_pair( "RelativeSpeedCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), - std::make_pair("TraveledDistanceCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), - std::make_pair( "ReachPositionCondition", [&](const auto & node) { return make< ReachPositionCondition>(node, scope, triggering_entities); }), - std::make_pair( "DistanceCondition", [&](const auto & node) { return make< DistanceCondition>(node, scope, triggering_entities); }), - std::make_pair("RelativeDistanceCondition", [&](const auto & node) { return make(node, scope, triggering_entities); }) + std::make_pair( "EndOfRoadCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "CollisionCondition", [&](const auto & node) { return make< CollisionCondition>(node, scope, triggering_entities); }), + std::make_pair( "OffroadCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "TimeHeadwayCondition", [&](const auto & node) { return make< TimeHeadwayCondition>(node, scope, triggering_entities); }), + std::make_pair( "TimeToCollisionCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "AccelerationCondition", [&](const auto & node) { return make< AccelerationCondition>(node, scope, triggering_entities); }), + std::make_pair( "StandStillCondition", [&](const auto & node) { return make< StandStillCondition>(node, scope, triggering_entities); }), + std::make_pair( "SpeedCondition", [&](const auto & node) { return make< SpeedCondition>(node, scope, triggering_entities); }), + std::make_pair( "RelativeSpeedCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "TraveledDistanceCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "ReachPositionCondition", [&](const auto & node) { return make< ReachPositionCondition>(node, scope, triggering_entities); }), + std::make_pair( "DistanceCondition", [&](const auto & node) { return make< DistanceCondition>(node, scope, triggering_entities); }), + std::make_pair( "RelativeDistanceCondition", [&](const auto & node) { return make< RelativeDistanceCondition>(node, scope, triggering_entities); }), + std::make_pair("RelativeClearanceCondition", [&](const auto & node) { return make(node, scope, triggering_entities); }), + std::make_pair( "AngleCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "RelativeAngleCondition", [&](const auto & node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }) )) // clang-format on { diff --git a/openscenario/openscenario_interpreter/src/syntax/integer.cpp b/openscenario/openscenario_interpreter/src/syntax/integer.cpp index 2c7f55c03ea..e85244d50ac 100644 --- a/openscenario/openscenario_interpreter/src/syntax/integer.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/integer.cpp @@ -34,6 +34,16 @@ Integer::Integer(const std::string & s) } } +auto Integer::max() noexcept -> Integer +{ + return static_cast(std::numeric_limits::max()); +} + +auto Integer::min() noexcept -> Integer +{ + return static_cast(std::numeric_limits::min()); +} + auto Integer::operator+=(const double & rhs) -> Integer & { data += rhs; diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_clearance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_clearance_condition.cpp new file mode 100644 index 00000000000..5de182c649f --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/relative_clearance_condition.cpp @@ -0,0 +1,206 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +RelativeClearanceCondition::RelativeClearanceCondition( + const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) +: Scope(scope), + distance_backward(readAttribute("distanceBackward", node, scope, 0.0)), + distance_forward(readAttribute("distanceForward", node, scope, 0.0)), + free_space(readAttribute("freeSpace", node, scope)), + opposite_lanes(readAttribute("oppositeLanes", node, scope)), + relative_lane_range(readElements("RelativeLaneRange", node, scope)), + entity_refs(readElements("EntityRef", node, scope)), + triggering_entities(triggering_entities) +{ +} + +auto RelativeClearanceCondition::description() const -> String +{ + std::stringstream description; + + description << triggering_entities.description() << " have clearances to given entities ["; + + print_to(description, entity_refs); + + description << "] with relative longitudinal ranges ( forward: " << distance_forward + << ", backward: " << distance_backward << " ) and relative lateral ranges ["; + + if (relative_lane_range.empty()) { + description << "all lanes"; + } else { + for (auto range = relative_lane_range.begin(); range != relative_lane_range.end(); range++) { + description << ((range != relative_lane_range.begin()) ? std::string(", ") : std::string("")); + if (range->from) { + if (range->to) { + description << range->from.value() << " to " << range->to.value(); + } else { + description << range->from.value() << "or more"; + } + } else { + if (range->to) { + description << range->to.value() << " or less"; + } + } + } + } + + description << "] "; + + if (opposite_lanes) { + description << " including opposite lanes?"; + } else { + description << " excluding opposite lanes?"; + } + + return description.str(); +} + +auto RelativeClearanceCondition::evaluate() -> Object +{ + auto is_relative_clearance_exist = [&]( + const auto & triggering_entity, const auto & target_entity) { + auto is_back = + (std::abs(evaluateRelativeHeading(triggering_entity)) > + boost::math::constants::half_pi()); + auto is_in_lateral_range = [&]() { + // The lanes to be checked to left and right of the triggering entity (positive to the y-axis). If omitted: all lanes are checked. + if (relative_lane_range.empty()) { + return true; + } else { + int relative_lateral_lane; + try { + relative_lateral_lane = evaluateLateralRelativeLanes( + triggering_entity, target_entity, RoutingAlgorithm::shortest); + } catch (const std::exception &) { + // occurring errors means that the target entity is not in the specified range, + // under the assumption that relative lane range is defined in routable range . + return false; + } + if (is_back) { + relative_lateral_lane = -relative_lateral_lane; + } + return std::any_of( + relative_lane_range.begin(), relative_lane_range.end(), + [&](const auto & range) { return range.evaluate(relative_lateral_lane); }); + } + }; + + auto is_in_longitudinal_range = [&]() { + auto relative_longitudinal = [&]() { + if (free_space) { + return static_cast( + makeNativeBoundingBoxRelativeLanePosition( + triggering_entity, target_entity, RoutingAlgorithm::shortest)) + .s; + } else { + return static_cast( + makeNativeRelativeLanePosition( + triggering_entity, target_entity, RoutingAlgorithm::shortest)) + .s; + } + }(); + + if (is_back) { + relative_longitudinal = -relative_longitudinal; + } + if (relative_longitudinal < 0) { + return std::abs(relative_longitudinal) <= distance_backward; + } else { + return relative_longitudinal <= distance_forward; + } + }; + + auto lat_ok = is_in_lateral_range(); + auto lon_ok = is_in_longitudinal_range(); + return not(lat_ok && lon_ok); + }; + + return asBoolean(triggering_entities.apply([&](const auto & triggering_scenario_object) { + assert(triggering_scenario_object.is()); + if (not entity_refs.empty()) { + return std::all_of( + entity_refs.begin(), entity_refs.end(), [&](const auto & target_entity_ref) { + const auto & target_entity = global().entities->ref(target_entity_ref); + if (target_entity.template is()) { + return is_relative_clearance_exist( + triggering_scenario_object, target_entity.template as().name); + ; + } else if (target_entity.template is()) { // + auto target_scenario_objects = target_entity.template as().objects(); + return std::all_of( + target_scenario_objects.begin(), target_scenario_objects.end(), + [&](const auto & target_scenario_object) { + return is_relative_clearance_exist( + triggering_scenario_object, target_scenario_object.name()); + }); + } else { + throw common::Error( + "Unexpected entity interface is detected. RelativeClearanceCondition expects " + "ScenarioObject or EntitySelection."); + } + }); + } else { + return std::all_of( + global().entities->begin(), global().entities->end(), + [&](const auto & target_candidate_entity) { + if (not target_candidate_entity.second.template is()) { + return true; + } else if (not target_candidate_entity.second.template as().is_added) { + return true; + } else { + const ScenarioObject & target_candidate_scenario_object = + target_candidate_entity.second.template as(); + // exclude entities included in TriggeringEntities + auto is_target_candidate_scenario_object_in_triggering_entities = + [&](const ScenarioObject & target_candidate) -> bool { + bool is_included = false; + triggering_entities.apply( + [target_candidate, &is_included](const auto & triggering_scenario_object) { + assert(triggering_scenario_object.is()); + if (triggering_scenario_object.name() == target_candidate.name) { + is_included = true; + } + return true; + }); + return is_included; + }; + if (is_target_candidate_scenario_object_in_triggering_entities( + target_candidate_scenario_object)) { + return true; + } else { + const auto & target_scenario_object = target_candidate_scenario_object; + return is_relative_clearance_exist( + triggering_scenario_object, target_scenario_object.name); + } + } + }); + } + })); +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_lane_range.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_lane_range.cpp new file mode 100644 index 00000000000..cd5fbe32950 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/relative_lane_range.cpp @@ -0,0 +1,33 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +RelativeLaneRange::RelativeLaneRange(const pugi::xml_node & node, Scope & scope) +: from(readAttribute("from", node, scope, std::nullopt)), + to(readAttribute("to", node, scope, std::nullopt)) +{ +} + +auto RelativeLaneRange::evaluate(Integer::value_type value) const -> bool +{ + return (not from || from.value() <= value) && (not to || to >= value); +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/storyboard_element_state_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/storyboard_element_state_condition.cpp index bcdf30c71d8..5d8758f5ca1 100644 --- a/openscenario/openscenario_interpreter/src/syntax/storyboard_element_state_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/storyboard_element_state_condition.cpp @@ -38,7 +38,7 @@ StoryboardElementStateCondition::StoryboardElementStateCondition( Note that there is no guarantee that the StoryboardElement will remain in the same state as when the notification is made until StoryboardElementStateCondition::evaluate is executed after the callback - is called. The In other words, the StoryboardElement may immediately + is called. In other words, the StoryboardElement may immediately transition to the next state after calling the callback function. */ diff --git a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp index 480d04a2784..09107890eb9 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp @@ -32,47 +32,77 @@ namespace openscenario_interpreter inline namespace syntax { template -struct MagicSubscription : private rclcpp::Node, public T +struct MagicSubscription : private T { - std::promise promise; + struct Node : public rclcpp::Node + { + std::atomic_bool stop_requested = false; - std::thread thread; + std::mutex mutex; - std::exception_ptr thrown; + std::exception_ptr thrown; - typename rclcpp::Subscription::SharedPtr subscription; + std::thread thread; -public: - explicit MagicSubscription(const std::string & node_name, const std::string & topic_name) - : rclcpp::Node(node_name), - thread( - [this](auto future) { - while (rclcpp::ok() and - future.wait_for(std::chrono::milliseconds(1)) == std::future_status::timeout) { + explicit Node() + : rclcpp::Node("magic_subscription"), thread([this]() { + while (rclcpp::ok() and not stop_requested) { try { rclcpp::spin_some(get_node_base_interface()); } catch (...) { + auto lock = std::lock_guard(mutex); thrown = std::current_exception(); } } - }, - std::move(promise.get_future())), - subscription(create_subscription(topic_name, 1, [this](const typename T::SharedPtr message) { - static_cast(*this) = *message; - })) + }) + { + } + + ~Node() + { + if (thread.joinable() and not stop_requested.exchange(true)) { + thread.join(); + } + } + + auto rethrow() + { + if (auto lock = std::lock_guard(mutex); thrown) { + std::rethrow_exception(thrown); + } + } + }; + + static inline std::size_t count = 0; + + static inline std::unique_ptr node = nullptr; + + typename rclcpp::Subscription::SharedPtr subscription; + + explicit MagicSubscription(const std::string & topic_name) { + if (not count++) { + node = std::make_unique(); + } + + subscription = node->template create_subscription( + topic_name, rclcpp::QoS(1).best_effort(), + [this](const typename T::SharedPtr message) { static_cast(*this) = *message; }); } ~MagicSubscription() { - if (thread.joinable()) { - promise.set_value(); - thread.join(); + if (not --count) { + node.reset(); } } -}; -uint64_t UserDefinedValueCondition::magic_subscription_counter = 0; + auto get() const -> const auto & + { + node->rethrow(); + return static_cast(*this); + } +}; UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node, Scope & scope) : name(readAttribute("name", node, scope)), @@ -122,31 +152,29 @@ UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node using tier4_simulation_msgs::msg::UserDefinedValueType; evaluate_value = - [&, current_message = std::make_shared>( - result.str(1) + "_subscription_" + std::to_string(++magic_subscription_counter), - result.str(0))]() { - auto evaluate = [](const auto & user_defined_value) { - switch (user_defined_value.type.data) { + [this, subscriber = std::make_shared>(result.str(0))]() { + if (const auto message = subscriber->get(); message.value.empty()) { + return unspecified; + } else { + switch (message.type.data) { case UserDefinedValueType::BOOLEAN: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::DATE_TIME: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::DOUBLE: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::INTEGER: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::STRING: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::UNSIGNED_INT: - return make(user_defined_value.value); + return make(message.value); case UserDefinedValueType::UNSIGNED_SHORT: - return make(user_defined_value.value); + return make(message.value); default: return unspecified; } - }; - - return not current_message->value.empty() ? evaluate(*current_message) : unspecified; + } }; #else throw SyntaxError( diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 901a263cbcc..8d59f7075cf 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 21895c9830b..f3a8ec6938a 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 3.3.0 + 4.3.7 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index 0fe321e7b1f..11e155b554c 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index f61976af7e3..cda36efa5c2 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 3.3.0 + 4.3.7 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 9c9fa531704..70770786f58 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 9cf35811f3b..a7ac30ff5e2 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 3.3.0 + 4.3.7 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 7bbbed269b1..d21c15fc138 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 708ee18849f..c4ae76aa305 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 3.3.0 + 4.3.7 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index c0a9eaebdc8..b79a0ddcb91 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,271 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 8e6d26cf040..5fd2bb0016b 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 3.3.0 + 4.3.7 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 1a1e1c68113..7d28b5c16e7 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,270 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index e4494bd1ce3..d9615c20ec9 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 3.3.0 + 4.3.7 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 98236e8269a..96817064338 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 2ec12cb34a6..b894431dc2a 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 3.3.0 + 4.3.7 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 7e6e0955728..6973ea4d9d8 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index dcec7f66a56..aceb1fb02a7 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 3.3.0 + 4.3.7 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index e2c97861495..31db2269f5a 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,274 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ +* Spelling changes +* Lint changes +* Contributors: Grzegorz Maj + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 69374013ad2..521171dcb8d 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 3.3.0 + 4.3.7 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index dc80b692e40..ffa638f5e72 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,285 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge pull request `#1356 `_ from tier4/RJD-1056-remove-current-time-step-time + Remove unused data members: current_time step_time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Trailing return type +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Add const to time argument in behavior +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): remove unused variable +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): use CanonicalizedEntityStatus as shared_ptr inside BT and use ::set +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree_plugin): rename updated_status variable +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* fix(traffic_simulator, behavior_tree_plugin): fix returned EntityStatus from bt, fix canonicalize in vehicle_entity and pedestrian_entity +* feat(behavior_tree_plugin, entity_base): move toCanonicalizedEntityPose to traffic_simulator, use EntityStatus as updated_state in BehaviorTree +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index 37e1a9c06b8..09a3e029752 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -66,8 +66,6 @@ class ActionNode : public BT::ActionNodeBase -> std::vector; auto stopEntity() const -> void; auto getHorizon() const -> double; - auto getActionStatus() const noexcept -> traffic_simulator_msgs::msg::ActionStatus; - auto getEntityName() const noexcept -> std::string; /// throws if the derived class return RUNNING. auto executeTick() -> BT::NodeStatus override; @@ -85,11 +83,10 @@ class ActionNode : public BT::ActionNodeBase BT::InputPort("route_lanelets"), BT::InputPort>("target_speed"), BT::InputPort>("hdmap_utils"), - BT::InputPort>("entity_status"), + BT::InputPort>("canonicalized_entity_status"), BT::InputPort>("traffic_lights"), BT::InputPort("request"), BT::OutputPort>("obstacle"), - BT::OutputPort>("non_canonicalized_updated_status"), BT::OutputPort("waypoints"), BT::OutputPort("request"), // clang-format on @@ -103,6 +100,8 @@ class ActionNode : public BT::ActionNodeBase double width_extension_right = 0.0, double width_extension_left = 0.0, double length_extension_front = 0.0, double length_extension_rear = 0.0) const -> std::optional; + + auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) -> void; auto calculateUpdatedEntityStatus( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus; @@ -114,12 +113,11 @@ class ActionNode : public BT::ActionNodeBase traffic_simulator::behavior::Request request; std::shared_ptr hdmap_utils; std::shared_ptr traffic_lights; - std::shared_ptr entity_status; + std::shared_ptr canonicalized_entity_status; double current_time; double step_time; double default_matching_distance_for_lanelet_pose_calculation; std::optional target_speed; - std::shared_ptr non_canonicalized_updated_status; EntityStatusDict other_entity_status; lanelet::Ids route_lanelets; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp index 58503d2d9de..86d925e8a49 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp @@ -51,24 +51,23 @@ class PedestrianBehaviorTree : public BehaviorPluginBase // clang-format off DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr) DEFINE_GETTER_SETTER(CurrentTime, double) DEFINE_GETTER_SETTER(DebugMarker, std::vector) DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) DEFINE_GETTER_SETTER(GoalPoses, std::vector) - DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr) - DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) DEFINE_GETTER_SETTER(Obstacle, std::optional) DEFINE_GETTER_SETTER(OtherEntityStatus, EntityStatusDict) DEFINE_GETTER_SETTER(PedestrianParameters, traffic_simulator_msgs::msg::PedestrianParameters) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(ReferenceTrajectory, std::shared_ptr) DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request) DEFINE_GETTER_SETTER(RouteLanelets, lanelet::Ids) DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp index eae51146f2c..8333e5ae027 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp @@ -55,10 +55,10 @@ class VehicleBehaviorTree : public BehaviorPluginBase } // clang-format off + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr) DEFINE_GETTER_SETTER(CurrentTime, double) DEFINE_GETTER_SETTER(DebugMarker, std::vector) DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, double) - DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr) DEFINE_GETTER_SETTER(GoalPoses, std::vector) DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr) DEFINE_GETTER_SETTER(LaneChangeParameters, traffic_simulator::lane_change::Parameter) @@ -72,7 +72,6 @@ class VehicleBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index abfc5b34317..26b8b691172 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 3.3.0 + 4.3.7 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 64a7e072f36..dcb2cd9ae51 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -59,8 +59,8 @@ auto ActionNode::getBlackBoardValues() -> void THROW_SIMULATION_ERROR("failed to get input traffic_lights in ActionNode"); } if (!getInput>( - "entity_status", entity_status)) { - THROW_SIMULATION_ERROR("failed to get input entity_status in ActionNode"); + "canonicalized_entity_status", canonicalized_entity_status)) { + THROW_SIMULATION_ERROR("failed to get input canonicalized_entity_status in ActionNode"); } if (!getInput>("target_speed", target_speed)) { @@ -84,14 +84,21 @@ auto ActionNode::getBlackBoardValues() -> void auto ActionNode::getHorizon() const -> double { - return std::clamp(entity_status->getTwist().linear.x * 5.0, 20.0, 50.0); + return std::clamp(canonicalized_entity_status->getTwist().linear.x * 5.0, 20.0, 50.0); } auto ActionNode::stopEntity() const -> void { - entity_status->setTwist(geometry_msgs::msg::Twist()); - entity_status->setAccel(geometry_msgs::msg::Accel()); - entity_status->setLinearJerk(0); + canonicalized_entity_status->setTwist(geometry_msgs::msg::Twist()); + canonicalized_entity_status->setAccel(geometry_msgs::msg::Accel()); + canonicalized_entity_status->setLinearJerk(0); +} + +auto ActionNode::setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) + -> void +{ + canonicalized_entity_status->set( + entity_status, default_matching_distance_for_lanelet_pose_calculation, hdmap_utils); } auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const @@ -116,8 +123,8 @@ auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) c const auto right_of_way_ids = hdmap_utils->getRightOfWayLaneletIds(lanelet); for (const auto right_of_way_id : right_of_way_ids) { const auto other_status = getOtherEntityStatus(right_of_way_id); - if (!other_status.empty() && entity_status->isInLanelet()) { - const auto lanelet_pose = entity_status->getLaneletPose(); + if (!other_status.empty() && canonicalized_entity_status->isInLanelet()) { + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); const auto distance_forward = hdmap_utils->getLongitudinalDistance( lanelet_pose, traffic_simulator::helper::constructLaneletPose(lanelet, 0)); const auto distance_backward = hdmap_utils->getLongitudinalDistance( @@ -167,11 +174,12 @@ auto ActionNode::getRightOfWayEntities(const lanelet::Ids & following_lanelets) auto ActionNode::getRightOfWayEntities() const -> std::vector { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { return {}; } std::vector ret; - const auto lanelet_ids = hdmap_utils->getRightOfWayLaneletIds(entity_status->getLaneletId()); + const auto lanelet_ids = + hdmap_utils->getRightOfWayLaneletIds(canonicalized_entity_status->getLaneletId()); if (lanelet_ids.empty()) { return ret; } @@ -235,7 +243,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf for (const auto & each : other_entity_status) { const auto distance = getDistanceToTargetEntityPolygon(spline, each.first); const auto quat = math::geometry::getRotation( - entity_status->getMapPose().orientation, + canonicalized_entity_status->getMapPose().orientation, other_entity_status.at(each.first).getMapPose().orientation); /** * @note hard-coded parameter, if the Yaw value of RPY is in ~1.5708 -> 1.5708, entity is a candidate of front entity. @@ -395,26 +403,30 @@ auto ActionNode::calculateUpdatedEntityStatus( { const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( - step_time, getEntityName()); + step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, entity_status->getTwist(), entity_status->getAccel()); + target_speed, constraints, canonicalized_entity_status->getTwist(), + canonicalized_entity_status->getAccel()); const double linear_jerk_new = std::get<2>(dynamics); const geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); const geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics); - if (!entity_status->isInLanelet()) { - THROW_SIMULATION_ERROR("Entity ", entity_status->getName(), " is not matched to the lanelet."); + if (!canonicalized_entity_status->isInLanelet()) { + THROW_SIMULATION_ERROR( + "Entity ", canonicalized_entity_status->getName(), " is not matched to the lanelet."); } else { - auto lanelet_pose = entity_status->getLaneletPose(); + auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); lanelet_pose.s = - lanelet_pose.s + (twist_new.linear.x + entity_status->getTwist().linear.x) / 2.0 * step_time; + lanelet_pose.s + + (twist_new.linear.x + canonicalized_entity_status->getTwist().linear.x) / 2.0 * step_time; const auto canonicalized = hdmap_utils->canonicalizeLaneletPose(lanelet_pose, route_lanelets); if ( const auto canonicalized_lanelet_pose = std::get>(canonicalized)) { // If canonicalize succeed, set canonicalized pose and set other values. - auto entity_status_updated = static_cast(*entity_status); + auto entity_status_updated = + static_cast(*canonicalized_entity_status); { entity_status_updated.time = current_time + step_time; entity_status_updated.lanelet_pose = canonicalized_lanelet_pose.value(); @@ -437,7 +449,8 @@ auto ActionNode::calculateUpdatedEntityStatus( end_of_road_lanelet_pose.offset = lanelet_pose.offset; end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; } - auto entity_status_updated = static_cast(*entity_status); + auto entity_status_updated = + static_cast(*canonicalized_entity_status); { entity_status_updated.time = current_time + step_time; entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; @@ -457,7 +470,8 @@ auto ActionNode::calculateUpdatedEntityStatus( end_of_road_lanelet_pose.offset = lanelet_pose.offset; end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; } - auto entity_status_updated = static_cast(*entity_status); + auto entity_status_updated = + static_cast(*canonicalized_entity_status); { entity_status_updated.time = current_time + step_time; entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; @@ -483,9 +497,10 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( using math::geometry::operator*; const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( - step_time, getEntityName()); + step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, entity_status->getTwist(), entity_status->getAccel()); + target_speed, constraints, canonicalized_entity_status->getTwist(), + canonicalized_entity_status->getAccel()); double linear_jerk_new = std::get<2>(dynamics); geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics); @@ -494,17 +509,18 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( angular_trans_vec.z = twist_new.angular.z * step_time; geometry_msgs::msg::Quaternion angular_trans_quat = math::geometry::convertEulerAngleToQuaternion(angular_trans_vec); - pose_new.orientation = entity_status->getMapPose().orientation * angular_trans_quat; + pose_new.orientation = canonicalized_entity_status->getMapPose().orientation * angular_trans_quat; Eigen::Vector3d trans_vec; trans_vec(0) = twist_new.linear.x * step_time; trans_vec(1) = twist_new.linear.y * step_time; trans_vec(2) = 0; Eigen::Matrix3d rotation_mat = math::geometry::getRotationMatrix(pose_new.orientation); trans_vec = rotation_mat * trans_vec; - pose_new.position.x = trans_vec(0) + entity_status->getMapPose().position.x; - pose_new.position.y = trans_vec(1) + entity_status->getMapPose().position.y; - pose_new.position.z = trans_vec(2) + entity_status->getMapPose().position.z; - auto entity_status_updated = static_cast(*entity_status); + pose_new.position.x = trans_vec(0) + canonicalized_entity_status->getMapPose().position.x; + pose_new.position.y = trans_vec(1) + canonicalized_entity_status->getMapPose().position.y; + pose_new.position.z = trans_vec(2) + canonicalized_entity_status->getMapPose().position.z; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); entity_status_updated.time = current_time + step_time; entity_status_updated.lanelet_pose = traffic_simulator::LaneletPose(); entity_status_updated.lanelet_pose_valid = false; @@ -519,19 +535,9 @@ auto ActionNode::calculateStopDistance( const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> double { return traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( - step_time, getEntityName()) + step_time, canonicalized_entity_status->getName()) .getRunningDistance( - 0, constraints, entity_status->getTwist(), entity_status->getAccel(), - entity_status->getLinearJerk()); -} - -auto ActionNode::getActionStatus() const noexcept -> traffic_simulator_msgs::msg::ActionStatus -{ - return static_cast(*entity_status).action_status; -} - -auto ActionNode::getEntityName() const noexcept -> std::string -{ - return static_cast(*entity_status).name; + 0, constraints, canonicalized_entity_status->getTwist(), + canonicalized_entity_status->getAccel(), canonicalized_entity_status->getLinearJerk()); } } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp index 034a6209feb..773f2fcceba 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp @@ -38,21 +38,16 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { stopEntity(); - setOutput( - "non_canonicalized_updated_status", - std::make_shared( - static_cast(*entity_status))); return BT::NodeStatus::RUNNING; } - auto following_lanelets = hdmap_utils->getFollowingLanelets(entity_status->getLaneletId()); + auto following_lanelets = + hdmap_utils->getFollowingLanelets(canonicalized_entity_status->getLaneletId()); if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(following_lanelets); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); return BT::NodeStatus::RUNNING; } } // namespace pedestrian diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index ec6920082ae..8e722f5afb0 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -22,7 +22,7 @@ auto FollowPolylineTrajectoryAction::calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { auto waypoints = traffic_simulator_msgs::msg::WaypointsArray(); - waypoints.waypoints.push_back(entity_status->getMapPose().position); + waypoints.waypoints.push_back(canonicalized_entity_status->getMapPose().position); for (const auto & vertex : polyline_trajectory->shape.vertices) { waypoints.waypoints.push_back(vertex.position.position); } @@ -60,30 +60,26 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus if (target_speed.has_value()) { return target_speed.value(); } else { - return entity_status->getTwist().linear.x; + return canonicalized_entity_status->getTwist().linear.x; } }; - auto getMatchingDistance = [&]() -> double { - return entity_status->getBoundingBox().dimensions.y * 0.5 + 1.0; - }; - if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput("polyline_trajectory", polyline_trajectory) or not getInput("target_speed", target_speed) or not polyline_trajectory) { return BT::NodeStatus::FAILURE; - } else if (std::isnan(entity_status->getTime())) { + } else if (std::isnan(canonicalized_entity_status->getTime())) { THROW_SIMULATION_ERROR( - "Time in entity_status is NaN - FollowTrajectoryAction does not support such case."); + "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such " + "case."); } else if ( - const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) { - setOutput( - "non_canonicalized_updated_status", - std::make_shared(updated_status.value())); + const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( + static_cast(*canonicalized_entity_status), + *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { + setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index 57b05a73540..cbf2f304c22 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -51,20 +51,21 @@ auto PedestrianActionNode::calculateUpdatedEntityStatus(double target_speed) con auto PedestrianActionNode::calculateUpdatedEntityStatusInWorldFrame(double target_speed) const -> traffic_simulator::EntityStatus { - auto updated_status = ActionNode::calculateUpdatedEntityStatusInWorldFrame( + auto entity_status_updated = ActionNode::calculateUpdatedEntityStatusInWorldFrame( target_speed, behavior_parameter.dynamic_constraints); if ( const auto canonicalized_lanelet_pose = traffic_simulator::pose::pedestrian::transformToCanonicalizedLaneletPose( - updated_status.pose, entity_status->getBoundingBox(), entity_status->getLaneletIds(), true, + entity_status_updated.pose, canonicalized_entity_status->getBoundingBox(), + canonicalized_entity_status->getLaneletIds(), true, default_matching_distance_for_lanelet_pose_calculation, hdmap_utils)) { - updated_status.lanelet_pose_valid = true; - updated_status.lanelet_pose = + entity_status_updated.lanelet_pose_valid = true; + entity_status_updated.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); } else { - updated_status.lanelet_pose_valid = false; - updated_status.lanelet_pose = traffic_simulator::LaneletPose(); + entity_status_updated.lanelet_pose_valid = false; + entity_status_updated.lanelet_pose = traffic_simulator::LaneletPose(); } - return updated_status; + return entity_status_updated; } } // namespace entity_behavior diff --git a/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp index c9d6ca6b4a1..e4b8f5f3a95 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/walk_straight_action.cpp @@ -47,10 +47,7 @@ BT::NodeStatus WalkStraightAction::tick() if (!target_speed) { target_speed = 1.111; } - setOutput( - "non_canonicalized_updated_status", - std::make_shared( - calculateUpdatedEntityStatusInWorldFrame(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatusInWorldFrame(target_speed.value())); return BT::NodeStatus::RUNNING; } } // namespace pedestrian diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp index ef15f31dd72..2697bd8bdb1 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp @@ -52,13 +52,13 @@ FollowFrontEntityAction::calculateObstacle(const traffic_simulator_msgs::msg::Wa const traffic_simulator_msgs::msg::WaypointsArray FollowFrontEntityAction::calculateWaypoints() { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -117,9 +117,7 @@ BT::NodeStatus FollowFrontEntityAction::tick() } const double front_entity_linear_velocity = front_entity_status.getTwist().linear.x; if (target_speed.value() <= front_entity_linear_velocity) { - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto obstacle = calculateObstacle(waypoints); setOutput("waypoints", waypoints); setOutput("obstacle", obstacle); @@ -129,28 +127,19 @@ BT::NodeStatus FollowFrontEntityAction::tick() distance_to_front_entity_.value() >= (calculateStopDistance(behavior_parameter.dynamic_constraints) + vehicle_parameters.bounding_box.dimensions.x + 5)) { - setOutput( - "non_canonicalized_updated_status", - std::make_shared( - calculateUpdatedEntityStatus(front_entity_linear_velocity + 2))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity + 2)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; } else if ( distance_to_front_entity_.value() <= calculateStopDistance(behavior_parameter.dynamic_constraints)) { - setOutput( - "non_canonicalized_updated_status", - std::make_shared( - calculateUpdatedEntityStatus(front_entity_linear_velocity - 2))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity - 2)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; } else { - setOutput( - "non_canonicalized_updated_status", - std::make_shared( - calculateUpdatedEntityStatus(front_entity_linear_velocity))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(front_entity_linear_velocity)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp index 76517e78a37..b8d5e00e84e 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp @@ -39,12 +39,12 @@ const std::optional FollowLaneAction::cal const traffic_simulator_msgs::msg::WaypointsArray FollowLaneAction::calculateWaypoints() { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -74,12 +74,8 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { stopEntity(); - setOutput( - "non_canonicalized_updated_status", - std::make_shared( - static_cast(*entity_status))); return BT::NodeStatus::RUNNING; } const auto waypoints = calculateWaypoints(); @@ -132,9 +128,7 @@ BT::NodeStatus FollowLaneAction::tick() if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp index 8eb366bdb34..35a3469d377 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp @@ -35,13 +35,13 @@ const std::optional MoveBackwardAction::c const traffic_simulator_msgs::msg::WaypointsArray MoveBackwardAction::calculateWaypoints() { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { return traffic_simulator_msgs::msg::WaypointsArray(); } - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); const auto ids = hdmap_utils->getPreviousLanelets(lanelet_pose.lanelet_id); // DIFFERENT SPLINE - recalculation needed math::geometry::CatmullRomSpline spline(hdmap_utils->getCenterPoints(ids)); @@ -70,7 +70,7 @@ BT::NodeStatus MoveBackwardAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } const auto waypoints = calculateWaypoints(); @@ -78,12 +78,11 @@ BT::NodeStatus MoveBackwardAction::tick() return BT::NodeStatus::FAILURE; } if (!target_speed) { - target_speed = - hdmap_utils->getSpeedLimit(hdmap_utils->getPreviousLanelets(entity_status->getLaneletId())); + target_speed = hdmap_utils->getSpeedLimit( + hdmap_utils->getPreviousLanelets(canonicalized_entity_status->getLaneletId())); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp index 54377e9b086..cdc751475e4 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp @@ -54,12 +54,12 @@ StopAtCrossingEntityAction::calculateObstacle(const traffic_simulator_msgs::msg: const traffic_simulator_msgs::msg::WaypointsArray StopAtCrossingEntityAction::calculateWaypoints() { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -92,7 +92,7 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { in_stop_sequence_ = false; return BT::NodeStatus::FAILURE; } @@ -132,14 +132,12 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() } std::optional target_linear_speed; if (distance_to_stop_target_) { - target_linear_speed = calculateTargetSpeed(entity_status->getTwist().linear.x); + target_linear_speed = calculateTargetSpeed(canonicalized_entity_status->getTwist().linear.x); } else { target_linear_speed = std::nullopt; } if (!distance_to_stop_target_) { - setOutput( - "non_canonicalized_updated_status", - std::make_shared(calculateUpdatedEntityStatus(0))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); in_stop_sequence_ = false; @@ -152,9 +150,7 @@ BT::NodeStatus StopAtCrossingEntityAction::tick() } else { target_speed = target_linear_speed.value(); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); in_stop_sequence_ = true; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp index 949a6ff1e0c..74cc2ee182e 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp @@ -53,13 +53,13 @@ const std::optional StopAtStopLineAction: const traffic_simulator_msgs::msg::WaypointsArray StopAtStopLineAction::calculateWaypoints() { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -128,7 +128,7 @@ BT::NodeStatus StopAtStopLineAction::tick() } } - if (std::fabs(entity_status->getTwist().linear.x) < 0.001) { + if (std::fabs(canonicalized_entity_status->getTwist().linear.x) < 0.001) { if (distance_to_stopline_) { if (distance_to_stopline_.value() <= vehicle_parameters.bounding_box.dimensions.x + 5) { stopped_ = true; @@ -141,21 +141,17 @@ BT::NodeStatus StopAtStopLineAction::tick() } if (!distance_to_stopline_) { stopped_ = false; - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::SUCCESS; } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::RUNNING; } - auto target_linear_speed = calculateTargetSpeed(entity_status->getTwist().linear.x); + auto target_linear_speed = calculateTargetSpeed(canonicalized_entity_status->getTwist().linear.x); if (!target_linear_speed) { stopped_ = false; return BT::NodeStatus::FAILURE; @@ -167,9 +163,7 @@ BT::NodeStatus StopAtStopLineAction::tick() } else { target_speed = target_linear_speed.value(); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); stopped_ = false; setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp index 96e4b0b3347..269376945e0 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_traffic_light_action.cpp @@ -52,12 +52,12 @@ StopAtTrafficLightAction::calculateObstacle(const traffic_simulator_msgs::msg::W const traffic_simulator_msgs::msg::WaypointsArray StopAtTrafficLightAction::calculateWaypoints() { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + getHorizon(), 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -92,7 +92,7 @@ BT::NodeStatus StopAtTrafficLightAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } if (!behavior_parameter.see_around) { @@ -114,14 +114,12 @@ BT::NodeStatus StopAtTrafficLightAction::tick() if (distance_to_stop_target_.value() > getHorizon()) { return BT::NodeStatus::FAILURE; } - target_linear_speed = calculateTargetSpeed(entity_status->getTwist().linear.x); + target_linear_speed = calculateTargetSpeed(canonicalized_entity_status->getTwist().linear.x); } else { return BT::NodeStatus::FAILURE; } if (!distance_to_stop_target_) { - setOutput( - "non_canonicalized_updated_status", - std::make_shared(calculateUpdatedEntityStatus(0))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(0)); setOutput("waypoints", waypoints); setOutput("obstacle", calculateObstacle(waypoints)); return BT::NodeStatus::SUCCESS; @@ -133,9 +131,7 @@ BT::NodeStatus StopAtTrafficLightAction::tick() } else { target_speed = target_linear_speed.value(); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto obstacle = calculateObstacle(waypoints); setOutput("waypoints", waypoints); setOutput("obstacle", obstacle); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp index 5cc359cd7d2..25caedbdcca 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/yield_action.cpp @@ -52,13 +52,13 @@ const std::optional YieldAction::calculat const traffic_simulator_msgs::msg::WaypointsArray YieldAction::calculateWaypoints() { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { THROW_SIMULATION_ERROR("failed to assign lane"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); waypoints.waypoints = reference_trajectory->getTrajectory( lanelet_pose.s, lanelet_pose.s + horizon, 1.0, lanelet_pose.offset); trajectory = std::make_unique( @@ -82,7 +82,7 @@ std::optional YieldAction::calculateTargetSpeed() if (rest_distance < calculateStopDistance(behavior_parameter.dynamic_constraints)) { return 0; } - return entity_status->getTwist().linear.x; + return canonicalized_entity_status->getTwist().linear.x; } BT::NodeStatus YieldAction::tick() @@ -96,7 +96,7 @@ BT::NodeStatus YieldAction::tick() if (!behavior_parameter.see_around) { return BT::NodeStatus::FAILURE; } - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } const auto right_of_way_entities = getRightOfWayEntities(route_lanelets); @@ -104,9 +104,7 @@ BT::NodeStatus YieldAction::tick() if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto waypoints = calculateWaypoints(); if (waypoints.waypoints.empty()) { return BT::NodeStatus::FAILURE; @@ -121,9 +119,7 @@ BT::NodeStatus YieldAction::tick() if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } - setOutput( - "non_canonicalized_updated_status", std::make_shared( - calculateUpdatedEntityStatus(target_speed.value()))); + setCanonicalizedEntityStatus(calculateUpdatedEntityStatus(target_speed.value())); const auto waypoints = calculateWaypoints(); if (waypoints.waypoints.empty()) { return BT::NodeStatus::FAILURE; diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 98be60d6e81..c2c097a0da2 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -22,7 +22,7 @@ auto FollowPolylineTrajectoryAction::calculateWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { auto waypoints = traffic_simulator_msgs::msg::WaypointsArray(); - waypoints.waypoints.push_back(entity_status->getMapPose().position); + waypoints.waypoints.push_back(canonicalized_entity_status->getMapPose().position); for (const auto & vertex : polyline_trajectory->shape.vertices) { waypoints.waypoints.push_back(vertex.position.position); } @@ -60,34 +60,26 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus if (target_speed.has_value()) { return target_speed.value(); } else { - return entity_status->getTwist().linear.x; + return canonicalized_entity_status->getTwist().linear.x; } }; - auto getMatchingDistance = [&]() -> double { - return std::max( - vehicle_parameters.axles.front_axle.track_width, - vehicle_parameters.axles.rear_axle.track_width) * - 0.5 + - 1.0; - }; - if (getBlackBoardValues(); request != traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY or not getInput("polyline_trajectory", polyline_trajectory) or not getInput("target_speed", target_speed) or not polyline_trajectory) { return BT::NodeStatus::FAILURE; - } else if (std::isnan(entity_status->getTime())) { + } else if (std::isnan(canonicalized_entity_status->getTime())) { THROW_SIMULATION_ERROR( - "Time in entity_status is NaN - FollowTrajectoryAction does not support such case."); + "Time in canonicalized_entity_status is NaN - FollowTrajectoryAction does not support such " + "case."); } else if ( - const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, hdmap_utils, step_time, getMatchingDistance(), getTargetSpeed())) { - setOutput( - "non_canonicalized_updated_status", - std::make_shared(updated_status.value())); + const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( + static_cast(*canonicalized_entity_status), + *polyline_trajectory, behavior_parameter, hdmap_utils, step_time, + default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) { + setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); return BT::NodeStatus::RUNNING; diff --git a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp index c5f1f154732..bca07a4fa1f 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp @@ -46,7 +46,7 @@ const traffic_simulator_msgs::msg::WaypointsArray LaneChangeAction::calculateWay if (!lane_change_parameters_) { THROW_SIMULATION_ERROR("lane change parameter is null"); } - if (entity_status->getTwist().linear.x >= 0) { + if (canonicalized_entity_status->getTwist().linear.x >= 0) { traffic_simulator_msgs::msg::WaypointsArray waypoints; double horizon = getHorizon(); auto following_lanelets = @@ -102,10 +102,10 @@ BT::NodeStatus LaneChangeAction::tick() } if (!curve_) { if (request == traffic_simulator::behavior::Request::LANE_CHANGE) { - if (!entity_status->isInLanelet()) { + if (!canonicalized_entity_status->isInLanelet()) { return BT::NodeStatus::FAILURE; } - const auto lanelet_pose = entity_status->getLaneletPose(); + const auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); if (!hdmap_utils->canChangeLane( lanelet_pose.lanelet_id, lane_change_parameters_->target.lanelet_id)) { return BT::NodeStatus::FAILURE; @@ -156,14 +156,14 @@ BT::NodeStatus LaneChangeAction::tick() .position.y); switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: - lane_change_velocity_ = entity_status->getTwist().linear.x; + lane_change_velocity_ = canonicalized_entity_status->getTwist().linear.x; break; case traffic_simulator::lane_change::Constraint::Type::LATERAL_VELOCITY: lane_change_velocity_ = curve_->getLength() / (offset / lane_change_parameters_->constraint.value); break; case traffic_simulator::lane_change::Constraint::Type::LONGITUDINAL_DISTANCE: - lane_change_velocity_ = entity_status->getTwist().linear.x; + lane_change_velocity_ = canonicalized_entity_status->getTwist().linear.x; break; case traffic_simulator::lane_change::Constraint::Type::TIME: lane_change_velocity_ = curve_->getLength() / lane_change_parameters_->constraint.value; @@ -181,17 +181,18 @@ BT::NodeStatus LaneChangeAction::tick() * @brief Force changing speed in order to fulfill constraint. */ case traffic_simulator::lane_change::Constraint::Policy::FORCE: - entity_status->setTwist(geometry_msgs::msg::Twist()); - entity_status->setAccel(geometry_msgs::msg::Accel()); - entity_status->setLinearVelocity(lane_change_velocity_); - current_s_ = current_s_ + entity_status->getTwist().linear.x * step_time; + canonicalized_entity_status->setTwist(geometry_msgs::msg::Twist()); + canonicalized_entity_status->setAccel(geometry_msgs::msg::Accel()); + canonicalized_entity_status->setLinearVelocity(lane_change_velocity_); + current_s_ = current_s_ + canonicalized_entity_status->getTwist().linear.x * step_time; break; /** * @brief Changing linear speed and try to fulfill constraint. */ case traffic_simulator::lane_change::Constraint::Policy::BEST_EFFORT: - target_accel = (lane_change_velocity_ - entity_status->getTwist().linear.x) / step_time; - if (entity_status->getTwist().linear.x > target_speed) { + target_accel = + (lane_change_velocity_ - canonicalized_entity_status->getTwist().linear.x) / step_time; + if (canonicalized_entity_status->getTwist().linear.x > target_speed) { target_accel = std::clamp( target_accel, behavior_parameter.dynamic_constraints.max_deceleration * -1.0, 0.0); } else { @@ -205,27 +206,26 @@ BT::NodeStatus LaneChangeAction::tick() * @note Hard coded parameter, -10.0 is a minimum linear velocity of the entity. */ twist_new.linear.x = std::clamp( - entity_status->getTwist().linear.x + accel_new.linear.x * step_time, -10.0, + canonicalized_entity_status->getTwist().linear.x + accel_new.linear.x * step_time, -10.0, vehicle_parameters.performance.max_speed); twist_new.linear.y = 0.0; twist_new.linear.z = 0.0; twist_new.angular.x = 0.0; twist_new.angular.y = 0.0; twist_new.angular.z = 0.0; - entity_status->setTwist(twist_new); - entity_status->setAccel(accel_new); - current_s_ = current_s_ + entity_status->getTwist().linear.x * step_time; + canonicalized_entity_status->setTwist(twist_new); + canonicalized_entity_status->setAccel(accel_new); + current_s_ = current_s_ + canonicalized_entity_status->getTwist().linear.x * step_time; break; } if (current_s_ < curve_->getLength()) { geometry_msgs::msg::Pose pose = curve_->getPose(current_s_, true); - auto entity_status_updated = static_cast(*entity_status); + auto entity_status_updated = + static_cast(*canonicalized_entity_status); entity_status_updated.pose = pose; entity_status_updated.lanelet_pose_valid = false; - entity_status_updated.action_status = getActionStatus(); - setOutput( - "non_canonicalized_updated_status", - std::make_shared(entity_status_updated)); + entity_status_updated.action_status = canonicalized_entity_status->getActionStatus(); + setCanonicalizedEntityStatus(entity_status_updated); const auto waypoints = calculateWaypoints(); if (waypoints.waypoints.empty()) { return BT::NodeStatus::FAILURE; @@ -246,7 +246,8 @@ BT::NodeStatus LaneChangeAction::tick() curve_ = std::nullopt; current_s_ = 0; lane_change_velocity_ = 0; - auto entity_status_updated = static_cast(*entity_status); + auto entity_status_updated = + static_cast(*canonicalized_entity_status); traffic_simulator::LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lane_change_parameters_->target.lanelet_id; lanelet_pose.s = s; @@ -254,10 +255,8 @@ BT::NodeStatus LaneChangeAction::tick() entity_status_updated.lanelet_pose = lanelet_pose; entity_status_updated.lanelet_pose_valid = true; entity_status_updated.pose = hdmap_utils->toMapPose(lanelet_pose).pose; - entity_status_updated.action_status = getActionStatus(); - setOutput( - "non_canonicalized_updated_status", - std::make_shared(entity_status_updated)); + entity_status_updated.action_status = canonicalized_entity_status->getActionStatus(); + setCanonicalizedEntityStatus(entity_status_updated); return BT::NodeStatus::SUCCESS; } } diff --git a/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp b/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp index 6dc4ef3a711..549ba448d97 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp @@ -56,7 +56,7 @@ auto VehicleActionNode::calculateUpdatedEntityStatusInWorldFrame(double target_s if (target_speed > vehicle_parameters.performance.max_speed) { target_speed = vehicle_parameters.performance.max_speed; } else { - target_speed = entity_status->getTwist().linear.x; + target_speed = canonicalized_entity_status->getTwist().linear.x; } return ActionNode::calculateUpdatedEntityStatusInWorldFrame( target_speed, behavior_parameter.dynamic_constraints); diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index f5bb4e87dde..5f85aa7d9e5 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,283 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge pull request `#1354 `_ from tier4/fix/follow_trajectory + Fix follow trajectory action and timestamp in entity status +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* add angular velocity / acceleration as z axis value +* enabel calculate angular accel and jerk +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* fix interpolate ration calculation +* Contributors: Masaya Kataoka, Tatsuya Yamasaki + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): use CanonicalizedEntityStatus as shared_ptr inside BT and use ::set +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(do_notching_plugin): remove unecessary comment +* feat(behavior_tree_plugin, entity_base): move toCanonicalizedEntityPose to traffic_simulator, use EntityStatus as updated_state in BehaviorTree +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp index 57b0bbc5191..1804932d741 100644 --- a/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp +++ b/simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp @@ -70,14 +70,13 @@ public: \ private: \ TYPE FIELD_NAME; // clang-format off - DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter, behavior_parameter_) - DEFINE_GETTER_SETTER(CurrentTime, double, current_time_) - DEFINE_GETTER_SETTER(EntityStatus, std::shared_ptr, entity_status_) - DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr, hdmap_utils_) - DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr, polyline_trajectory) - DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request, request) - DEFINE_GETTER_SETTER(StepTime, double, step_time_) - DEFINE_GETTER_SETTER(UpdatedStatus, std::shared_ptr, updated_status_) + DEFINE_GETTER_SETTER(BehaviorParameter, traffic_simulator_msgs::msg::BehaviorParameter, behavior_parameter_) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, std::shared_ptr, canonicalized_entity_status_) + DEFINE_GETTER_SETTER(CurrentTime, double, current_time_) + DEFINE_GETTER_SETTER(HdMapUtils, std::shared_ptr, hdmap_utils_) + DEFINE_GETTER_SETTER(PolylineTrajectory, std::shared_ptr, polyline_trajectory) + DEFINE_GETTER_SETTER(Request, traffic_simulator::behavior::Request, request) + DEFINE_GETTER_SETTER(StepTime, double, step_time_) // clang-format on #undef DEFINE_GETTER_SETTER }; diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 1f09ab698fc..c56cb9459a1 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 3.3.0 + 4.3.7 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp index 278d43de5d1..a038a980e32 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -13,6 +13,8 @@ // limitations under the License. #include +#include +#include #include #include #include @@ -100,23 +102,36 @@ auto interpolateEntityStatusFromPolylineTrajectory( const auto linear_jerk = (entity_status->getAccel().linear.x - linear_acceleration) / (v1.time - v0.time); + const double angular_velocity = + math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(v0.position.orientation, v1.position.orientation)) + .z / + (v1.time - v0.time); + const auto angular_acceleration = + (entity_status->getTwist().angular.x - angular_velocity) / (v1.time - v0.time); + interpolated_entity_status.action_status.twist = geometry_msgs::build() .linear(geometry_msgs::build().x(linear_velocity).y(0).z(0)) - .angular(geometry_msgs::build().x(0).y(0).z(0)); + .angular(geometry_msgs::build().x(0).y(0).z(angular_velocity)); interpolated_entity_status.action_status.accel = geometry_msgs::build() .linear( geometry_msgs::build().x(linear_acceleration).y(0).z(0)) - .angular(geometry_msgs::build().x(0).y(0).z(0)); + .angular( + geometry_msgs::build().x(0).y(0).z(angular_acceleration)); interpolated_entity_status.action_status.linear_jerk = linear_jerk; return interpolated_entity_status; }; - if ((current_time + step_time) <= trajectory->shape.vertices.begin()->time) { + if ( + (current_time + step_time) <= + (trajectory->base_time + trajectory->shape.vertices.begin()->time)) { return std::nullopt; } - if (trajectory->shape.vertices.back().time <= (current_time + step_time)) { + if ( + (trajectory->base_time + trajectory->shape.vertices.back().time) <= + (current_time + step_time)) { return interpolate_entity_status( 1, *std::prev(trajectory->shape.vertices.end(), 2), *std::prev(trajectory->shape.vertices.end(), 1)); @@ -151,25 +166,28 @@ void DoNothingBehavior::update(double current_time, double step_time) if ( const auto interpolated_status = do_nothing_behavior::follow_trajectory::interpolateEntityStatusFromPolylineTrajectory( - getPolylineTrajectory(), getEntityStatus(), getCurrentTime(), getStepTime())) { - return std::make_shared(interpolated_status.value()); + getPolylineTrajectory(), getCanonicalizedEntityStatus(), getCurrentTime(), + getStepTime())) { + return interpolated_status.value(); } else { - return std::make_shared( - static_cast(*entity_status_)); + return static_cast(*canonicalized_entity_status_); } }; - entity_status_->setTime(current_time); + canonicalized_entity_status_->setTime(current_time); if (getRequest() == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { - setUpdatedStatus(interpolate_entity_status_on_polyline_trajectory()); + canonicalized_entity_status_->set( + interpolate_entity_status_on_polyline_trajectory(), + getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); if ( getCurrentTime() + getStepTime() >= do_nothing_behavior::follow_trajectory::getLastVertexTimestamp(getPolylineTrajectory())) { setRequest(traffic_simulator::behavior::Request::NONE); } } else { - setUpdatedStatus(std::make_shared( - static_cast(*entity_status_))); + canonicalized_entity_status_->set( + static_cast(*canonicalized_entity_status_), + getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); } } diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index fa1bc752f42..d54c0e2f26c 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,312 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge pull request `#1324 `_ from tier4/fix/fix-eigen-variable-definition + Fix/fix eigen variable definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* fix(simple_sensor_simulator): fix eigen variable definition +* Contributors: Masaya Kataoka, Mert Çolak + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Revert "feat(params): set use_sim_time default as True" + This reverts commit da85edf4956083563715daa3d60f0da1f94a423d. +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(params): set use_sim_time default as True +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge pull request `#1301 `_ from tier4/feature/simple_sensor_simulator_unit_tests_lidar + Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Removed dummy class + - Updated unit tests +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Added missed header file +* Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator + - Added unit tests to LidarSensor + - Addede unit tests to Primitive + - Refactored Raycaster unit tests +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge pull request `#1358 `_ from tier4/RJD-1056-remove-npc-logic-started + Remove unused data members: npc_logic_started +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* ref(ego_entity_simulation): slight improve - add const, rename current_time +* fix(ego_entity): fix autoware update when not npc_logic_started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(developer_guide, traffic_simulator): update doc after review changes, add code notes +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(developer_guide, pose utils): adopt lane_pose_calculation doc to canonicalization laneletpose in CanonicalizedEntityStatus +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(traffic_simulator): move toCanonicalizeLaneletPose to CanonicalizedEntityStatus::set, little tidy up +* ref(traffic_simulator): remove operator= for CanonicalizedEntityStatus, use set and assertions +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge branch 'master' into doc/longitudinal-control +* Merge pull request `#1321 `_ from tier4/feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* feat: Enhance IMU sensor configuration and initialization + - Added frame_id to ImuSensorConfiguration + - Separated noise standard deviations for orientation, twist, and acceleration + - Updated ImuSensorBase and ImuSensor classes for new noise distributions +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Fix an issue with an invalid namespace imu_link +* Fix an issue with an invalid namespace imu_link +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* feat(simple_sensor_simulator, imu): add gravity vector, tidy up +* feat(simulator_core, api, zmq): add attachImuSensor, add update imu sensors +* feat(simple_sensor_simulator): add imu_sensor +* Contributors: Dawid Moszynski, Koki Suzuki, Kotaro Yoshimoto, Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 2d2be34f7db..a66a436c362 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -35,6 +35,7 @@ include_directories( ament_auto_add_library(simple_sensor_simulator_component SHARED src/sensor_simulation/detection_sensor/detection_sensor.cpp src/sensor_simulation/lidar/lidar_sensor.cpp + src/sensor_simulation/imu/imu_sensor.cpp src/sensor_simulation/lidar/raycaster.cpp src/sensor_simulation/occupancy_grid/grid_traversal.cpp src/sensor_simulation/occupancy_grid/occupancy_grid_builder.cpp diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp new file mode 100644 index 00000000000..03a1c7313a7 --- /dev/null +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/imu/imu_sensor.hpp @@ -0,0 +1,113 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__IMU_SENSOR_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__IMU_SENSOR_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace simple_sensor_simulator +{ +class ImuSensorBase +{ +public: + explicit ImuSensorBase(const simulation_api_schema::ImuSensorConfiguration & configuration) + : add_gravity_(configuration.add_gravity()), + noise_standard_deviation_orientation_(configuration.noise_standard_deviation_orientation()), + noise_standard_deviation_twist_(configuration.noise_standard_deviation_twist()), + noise_standard_deviation_acceleration_(configuration.noise_standard_deviation_acceleration()), + random_generator_(configuration.use_seed() ? configuration.seed() : std::random_device{}()), + noise_distribution_orientation_(0.0, noise_standard_deviation_orientation_), + noise_distribution_twist_(0.0, noise_standard_deviation_twist_), + noise_distribution_acceleration_(0.0, noise_standard_deviation_acceleration_), + orientation_covariance_(calculateCovariance(noise_standard_deviation_orientation_)), + angular_velocity_covariance_(calculateCovariance(noise_standard_deviation_twist_)), + linear_acceleration_covariance_(calculateCovariance(noise_standard_deviation_acceleration_)) + { + } + + virtual ~ImuSensorBase() = default; + + virtual auto update( + const rclcpp::Time & current_ros_time, + const std::vector & statuses) const -> bool = 0; + +protected: + const bool add_gravity_; + const double noise_standard_deviation_orientation_; + const double noise_standard_deviation_twist_; + const double noise_standard_deviation_acceleration_; + mutable std::mt19937 random_generator_; + mutable std::normal_distribution<> noise_distribution_orientation_; + mutable std::normal_distribution<> noise_distribution_twist_; + mutable std::normal_distribution<> noise_distribution_acceleration_; + const std::array orientation_covariance_; + const std::array angular_velocity_covariance_; + const std::array linear_acceleration_covariance_; + + auto calculateCovariance(const double stddev) const -> std::array + { + return {std::pow(stddev, 2), 0, 0, 0, std::pow(stddev, 2), 0, 0, 0, std::pow(stddev, 2)}; + }; +}; + +template +class ImuSensor : public ImuSensorBase +{ +public: + explicit ImuSensor( + const simulation_api_schema::ImuSensorConfiguration & configuration, + const typename rclcpp::Publisher::SharedPtr & publisher) + : ImuSensorBase(configuration), + entity_name_(configuration.entity()), + frame_id_(configuration.frame_id()), + publisher_(publisher) + { + } + + auto update( + const rclcpp::Time & current_ros_time, + const std::vector & statuses) const -> bool override + { + for (const auto & status : statuses) { + if (status.name() == entity_name_) { + traffic_simulator_msgs::msg::EntityStatus status_msg; + simulation_interface::toMsg(status, status_msg); + publisher_->publish(generateMessage(current_ros_time, status_msg)); + return true; + } + } + return false; + } + +private: + auto generateMessage( + const rclcpp::Time & current_ros_time, + const traffic_simulator_msgs::msg::EntityStatus & status) const -> const MessageType; + + const std::string entity_name_; + const std::string frame_id_; + const typename rclcpp::Publisher::SharedPtr publisher_; +}; +} // namespace simple_sensor_simulator +#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__IMU_SENSOR_HPP_ diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 4dd0a6a3572..9e078dda845 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -100,12 +101,22 @@ class SensorSimulation node, configuration.architecture_type())); } + auto attachImuSensor( + const double /*current_simulation_time*/, + const simulation_api_schema::ImuSensorConfiguration & configuration, rclcpp::Node & node) + -> void + { + imu_sensors_.push_back(std::make_unique>( + configuration, node.create_publisher("/sensing/imu/imu_data", 1))); + } + auto updateSensorFrame( double current_simulation_time, const rclcpp::Time & current_ros_time, const std::vector &, const simulation_api_schema::UpdateTrafficLightsRequest &) -> void; private: + std::vector> imu_sensors_; std::vector> lidar_sensors_; std::vector> detection_sensors_; std::vector> occupancy_grid_sensors_; diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp index f79a38e4c7b..ff95f6fea7c 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/traffic_lights/traffic_lights_publisher.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TTRAFFIC_LIGHT_PUBLISHER_HPP_ -#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TTRAFFIC_LIGHT_PUBLISHER_HPP_ +#ifndef SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ #include #include @@ -53,4 +53,4 @@ class TrafficLightsPublisher : public TrafficLightsPublisherBase }; } // namespace traffic_lights } // namespace simple_sensor_simulator -#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TTRAFFIC_LIGHT_PUBLISHER_HPP_ +#endif // SIMPLE_SENSOR_SIMULATOR__SENSOR_SIMULATION__TRAFFIC_LIGHTS__TRAFFIC_LIGHTS_PUBLISHER_HPP_ diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp index ce7ac052d91..8d3f168c768 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp @@ -119,6 +119,9 @@ class ScenarioSimulator : public rclcpp::Node auto despawnEntity(const simulation_api_schema::DespawnEntityRequest &) -> simulation_api_schema::DespawnEntityResponse; + auto attachImuSensor(const simulation_api_schema::AttachImuSensorRequest &) + -> simulation_api_schema::AttachImuSensorResponse; + auto attachDetectionSensor(const simulation_api_schema::AttachDetectionSensorRequest &) -> simulation_api_schema::AttachDetectionSensorResponse; diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 1e0f85b5aba..f3a29312fc4 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 3.3.0 + 4.3.7 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp new file mode 100644 index 00000000000..9a86e1a3731 --- /dev/null +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/imu/imu_sensor.cpp @@ -0,0 +1,90 @@ +// Copyright 2024 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include +#include +#include +#include + +namespace simple_sensor_simulator +{ +template <> +auto ImuSensor::generateMessage( + const rclcpp::Time & current_ros_time, + const traffic_simulator_msgs::msg::EntityStatus & status) const -> const sensor_msgs::msg::Imu +{ + const auto applyNoise = + [&](geometry_msgs::msg::Vector3 & v, std::normal_distribution<> & distribution) { + v.x += distribution(random_generator_); + v.y += distribution(random_generator_); + v.z += distribution(random_generator_); + }; + + auto imu_msg = sensor_msgs::msg::Imu(); + imu_msg.header.stamp = current_ros_time; + imu_msg.header.frame_id = frame_id_; + + auto orientation_rpy = math::geometry::convertQuaternionToEulerAngle(status.pose.orientation); + auto twist = status.action_status.twist; + auto accel = status.action_status.accel; + + // Apply noise + if (noise_standard_deviation_orientation_ > 0.0) { + applyNoise(orientation_rpy, noise_distribution_orientation_); + } + if (noise_standard_deviation_twist_ > 0.0) { + applyNoise(twist.angular, noise_distribution_twist_); + } + if (noise_standard_deviation_acceleration_ > 0.0) { + applyNoise(accel.linear, noise_distribution_acceleration_); + } + + // Apply gravity + if (add_gravity_) { + tf2::Quaternion orientation_quaternion; + orientation_quaternion.setRPY(orientation_rpy.x, orientation_rpy.y, orientation_rpy.z); + tf2::Matrix3x3 rotation_matrix(orientation_quaternion); + tf2::Vector3 gravity(0.0, 0.0, -9.81); + tf2::Vector3 transformed_gravity = rotation_matrix * gravity; + accel.linear.x += transformed_gravity.x(); + accel.linear.y += transformed_gravity.y(); + accel.linear.z += transformed_gravity.z(); + } + + // Set data + imu_msg.orientation = math::geometry::convertEulerAngleToQuaternion(orientation_rpy); + imu_msg.angular_velocity.x = twist.angular.x; + imu_msg.angular_velocity.y = twist.angular.y; + imu_msg.angular_velocity.z = twist.angular.z; + imu_msg.linear_acceleration.x = accel.linear.x; + imu_msg.linear_acceleration.y = accel.linear.y; + imu_msg.linear_acceleration.z = accel.linear.z; + + // Set covariance + std::copy( + orientation_covariance_.begin(), orientation_covariance_.end(), + imu_msg.orientation_covariance.data()); + std::copy( + angular_velocity_covariance_.begin(), angular_velocity_covariance_.end(), + imu_msg.angular_velocity_covariance.data()); + std::copy( + linear_acceleration_covariance_.begin(), linear_acceleration_covariance_.end(), + imu_msg.linear_acceleration_covariance.data()); + return imu_msg; +} +} // namespace simple_sensor_simulator diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/sensor_simulation.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/sensor_simulation.cpp index 411d5c28f9a..f6f3716f62a 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/sensor_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/sensor_simulation.cpp @@ -24,8 +24,11 @@ auto SensorSimulation::updateSensorFrame( const std::vector & entities, const simulation_api_schema::UpdateTrafficLightsRequest & update_traffic_lights_request) -> void { - std::vector lidar_detected_objects = {}; + for (auto & sensor : imu_sensors_) { + sensor->update(current_ros_time, entities); + } + std::vector lidar_detected_objects = {}; for (auto & sensor : lidar_sensors_) { sensor->update(current_simulation_time, entities, current_ros_time); for (const auto & object : sensor->getDetectedObjects()) { diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index 43756c7579a..6ea8b184437 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -38,6 +38,7 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options) [this](auto &&... xs) { return spawnMiscObjectEntity(std::forward(xs)...); }, [this](auto &&... xs) { return despawnEntity(std::forward(xs)...); }, [this](auto &&... xs) { return updateEntityStatus(std::forward(xs)...); }, + [this](auto &&... xs) { return attachImuSensor(std::forward(xs)...); }, [this](auto &&... xs) { return attachLidarSensor(std::forward(xs)...); }, [this](auto &&... xs) { return attachDetectionSensor(std::forward(xs)...); }, [this](auto &&... xs) { return attachOccupancyGridSensor(std::forward(xs)...); }, @@ -298,6 +299,15 @@ auto ScenarioSimulator::despawnEntity(const simulation_api_schema::DespawnEntity return res; } +auto ScenarioSimulator::attachImuSensor(const simulation_api_schema::AttachImuSensorRequest & req) + -> simulation_api_schema::AttachImuSensorResponse +{ + sensor_sim_.attachImuSensor(current_simulation_time_, req.configuration(), *this); + auto res = simulation_api_schema::AttachImuSensorResponse(); + res.mutable_result()->set_success(true); + return res; +} + auto ScenarioSimulator::attachDetectionSensor( const simulation_api_schema::AttachDetectionSensorRequest & req) -> simulation_api_schema::AttachDetectionSensorResponse diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index ac4d22e3412..4ee28745a69 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -413,7 +413,7 @@ auto EgoEntitySimulation::getCurrentPose(const double pitch_angle = 0.) const -> geometry_msgs::msg::Pose { using math::geometry::operator*; - const auto relative_position = + const Eigen::Vector3d relative_position = math::geometry::getRotationMatrix(initial_pose_.orientation) * world_relative_position_; const auto relative_orientation = math::geometry::convertEulerAngleToQuaternion( geometry_msgs::build() diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index e7f6401dfef..5db9f69e675 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -1,3 +1,6 @@ +find_package(Protobuf REQUIRED) +include_directories(${Protobuf_INCLUDE_DIRS}) + add_subdirectory(src/sensor_simulation/lidar) add_subdirectory(src/sensor_simulation/primitives) add_subdirectory(src/sensor_simulation/occupancy_grid) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt index eeb9c2f9abc..7d16e7f14cf 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/CMakeLists.txt @@ -1,5 +1,5 @@ -find_package(Protobuf REQUIRED) -include_directories(${Protobuf_INCLUDE_DIRS}) - ament_add_gtest(test_raycaster test_raycaster.cpp) target_link_libraries(test_raycaster simple_sensor_simulator_component ${Protobuf_LIBRARIES}) + +ament_add_gtest(test_lidar_sensor test_lidar_sensor.cpp) +target_link_libraries(test_lidar_sensor simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp new file mode 100644 index 00000000000..2a225940738 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp @@ -0,0 +1,84 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_lidar_sensor.hpp" + +/** + * @note Test function behavior when called on a scene without Ego entity added - the goal is to + * test error throwing. + */ +TEST_F(LidarSensorTest, update_noEgo) +{ + status_.clear(); // Remove ego + EXPECT_THROW( + lidar_->update(current_simulation_time_, status_, current_ros_time_), std::runtime_error); +} + +/** + * @note Test basic functionality. Test lidar sensor correctness on a sample scene with some vehicle + * - the goal is to check if the correct pointcloud is published on the correct topic. + */ +TEST_F(LidarSensorTest, update_correct) +{ + lidar_->update(current_simulation_time_, status_, current_ros_time_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_NE(received_msg_, nullptr); + const auto total_num_of_points = received_msg_->width * received_msg_->height; + EXPECT_GT(total_num_of_points, 0); + EXPECT_EQ(received_msg_->header.frame_id, "base_link"); +} + +/** + * @note Test function behavior when called with a current_time significantly smaller than one call + * earlier - the goal is to test whether the function clears detected_objects. + */ +TEST_F(LidarSensorTest, update_goBackInTime) +{ + lidar_->update(current_simulation_time_, status_, rclcpp::Time(1000)); + + // Ensure there are detected objects + ASSERT_FALSE(lidar_->getDetectedObjects().empty()); + + lidar_->update(current_simulation_time_, status_, rclcpp::Time(1)); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + EXPECT_TRUE(lidar_->getDetectedObjects().empty()); +} + +/** + * @note Test basic functionality. Test detected objects obtaining from the statuses list containing + * Ego. + */ +TEST_F(LidarSensorTest, getDetectedObjects) +{ + const std::set expected_objects = {status_[1].name(), status_[2].name()}; + + lidar_->update(current_simulation_time_, status_, current_ros_time_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + const auto & detected_objects = lidar_->getDetectedObjects(); + + // LidarSensor returns duplicates. To avoid them, a std::set is used. + const std::set unique_objects(detected_objects.begin(), detected_objects.end()); + + ASSERT_FALSE(unique_objects.empty()); + EXPECT_EQ(unique_objects, expected_objects); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp new file mode 100644 index 00000000000..54bf61a03b0 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp @@ -0,0 +1,84 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +#include "../../utils/helper_functions.hpp" + +using namespace simple_sensor_simulator; + +class LidarSensorTest : public ::testing::Test +{ +protected: + LidarSensorTest() : config_(utils::constructLidarConfiguration("ego", "awf/universe", 0.0, 0.5)) + { + rclcpp::init(0, nullptr); + node_ = std::make_shared("lidar_sensor_test_node"); + makeRosInterface(); + initializeEntityStatuses(); + + lidar_ = std::make_unique>(0.0, config_, publisher_); + } + + ~LidarSensorTest() { rclcpp::shutdown(); } + + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::Subscription::SharedPtr subscription_; + + std::vector status_; + + std::unique_ptr lidar_; + simulation_api_schema::LidarConfiguration config_; + sensor_msgs::msg::PointCloud2::SharedPtr received_msg_; + + double current_simulation_time_{1.0}; + rclcpp::Time current_ros_time_{1}; + +private: + auto initializeEntityStatuses() -> void + { + const auto dimensions = utils::makeDimensions(4.5, 2.0, 1.5); + + const auto ego_status = utils::makeEntity( + "ego", EntityType::EGO, utils::makePose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0), dimensions); + const auto other1_status = utils::makeEntity( + "other1", EntityType::VEHICLE, utils::makePose(-3.0, -3.0, 0.0, 0.0, 0.0, 0.0, 1.0), + dimensions); + const auto other2_status = utils::makeEntity( + "other2", EntityType::VEHICLE, utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), + dimensions); + + status_ = {ego_status, other1_status, other2_status}; + } + + auto makeRosInterface() -> void + { + publisher_ = node_->create_publisher("lidar_output", 10); + subscription_ = node_->create_subscription( + "lidar_output", 10, + [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { received_msg_ = msg; }); + } +}; +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_LIDAR_SENSOR_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp index f159ea9b575..d168ec0c80b 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.cpp @@ -76,8 +76,8 @@ TEST_F(RaycasterTest, setDirection_oneBox) box_name_, box_depth_, box_width_, box_height_, box_pose_); simulation_api_schema::LidarConfiguration config; - config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring - config.set_horizontal_resolution(degToRad(1.0)); // Set horizontal resolution to 1 degree + config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring + config.set_horizontal_resolution(utils::degToRad(1.0)); // Set horizontal resolution to 1 degree raycaster_->setDirection(config); @@ -103,13 +103,7 @@ TEST_F(RaycasterTest, setDirection_manyBoxes) for (int i = 0; i < num_boxes; ++i) { const double angle = i * angle_increment; const auto box_pose = - geometry_msgs::build() - .position(geometry_msgs::build() - .x(radius * cos(angle)) - .y(radius * sin(angle)) - .z(0.0)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); + utils::makePose(radius * cos(angle), radius * sin(angle), 0.0, 0.0, 0.0, 0.0, 1.0); const std::string name = "box" + std::to_string(i); raycaster_->addPrimitive(name, box_depth_, box_width_, box_height_, box_pose); @@ -117,7 +111,7 @@ TEST_F(RaycasterTest, setDirection_manyBoxes) simulation_api_schema::LidarConfiguration config; config.add_vertical_angles(0.0); // Only one vertical angle for a horizontal ring - config.set_horizontal_resolution(degToRad(5.0)); + config.set_horizontal_resolution(utils::degToRad(5.0)); raycaster_->setDirection(config); diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp index d3e9beb511d..641e59cf104 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_raycaster.hpp @@ -23,6 +23,8 @@ #include #include +#include "../../utils/helper_functions.hpp" + using namespace simple_sensor_simulator; using namespace geometry_msgs::msg; @@ -31,20 +33,13 @@ constexpr static double degToRad(double deg) { return deg * M_PI / 180.0; } class RaycasterTest : public ::testing::Test { protected: - RaycasterTest() : raycaster_(std::make_unique()) + RaycasterTest() + : raycaster_(std::make_unique()), + config_(utils::constructLidarConfiguration("ego", "awf/universe", 0.0, 0.1)), + origin_(utils::makePose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)), + box_pose_(utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)) { - configureLidar(); - - origin_ = - geometry_msgs::build() - .position(geometry_msgs::build().x(0.0).y(0.0).z(0.0)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); - box_pose_ = - geometry_msgs::build() - .position(geometry_msgs::build().x(5.0).y(0.0).z(0.0)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0)); + raycaster_->setDirection(config_); } std::unique_ptr raycaster_; @@ -60,19 +55,6 @@ class RaycasterTest : public ::testing::Test geometry_msgs::msg::Pose origin_; geometry_msgs::msg::Pose box_pose_; - -private: - auto configureLidar() -> void - { - // Setting vertical angles from -15 to +15 degrees in 2 degree steps - for (double angle = -15.0; angle <= 15.0; angle += 2.0) { - config_.add_vertical_angles(degToRad(angle)); - } - // Setting horizontal resolution to 0.5 degrees - config_.set_horizontal_resolution(degToRad(0.5)); - - raycaster_->setDirection(config_); - } }; #endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_RAYCASTER_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt index fb424659262..b5ae088e7a7 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt @@ -3,3 +3,6 @@ target_link_libraries(test_box simple_sensor_simulator_component) ament_add_gtest(test_vertex test_vertex.cpp) target_link_libraries(test_vertex simple_sensor_simulator_component) + +ament_add_gtest(test_primitive test_primitive.cpp) +target_link_libraries(test_primitive simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp index c5b501075d7..25e64cf1022 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_box.cpp @@ -19,7 +19,7 @@ #include #include -#include "../expect_eq_macros.hpp" +#include "../../utils/expect_eq_macros.hpp" using namespace simple_sensor_simulator; using namespace simple_sensor_simulator::primitives; diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp new file mode 100644 index 00000000000..6438a0b2bb7 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.cpp @@ -0,0 +1,253 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_primitive.hpp" + +#include +#include +#include + +#include "../../utils/expect_eq_macros.hpp" + +/** + * @note Test basic functionality. Test adding to scene correctness with a sample primitive. + */ +TEST_F(PrimitiveTest, addToScene_sample) +{ + const std::vector expected_vertices = { + {0.0f, 1.0f, -1.0f}, {0.0f, 1.0f, 1.0f}, {0.0f, 3.0f, -1.0f}, {0.0f, 3.0f, 1.0f}, + {2.0f, 1.0f, -1.0f}, {2.0f, 1.0f, 1.0f}, {2.0f, 3.0f, -1.0f}, {2.0f, 3.0f, 1.0f}}; + const auto expected_triangles = primitive_->getTriangles(); + + RTCDevice device = rtcNewDevice(nullptr); + RTCScene scene = rtcNewScene(device); + + const unsigned int geom_id = primitive_->addToScene(device, scene); + ASSERT_NE(geom_id, RTC_INVALID_GEOMETRY_ID); + + const RTCGeometry geom = rtcGetGeometry(scene, geom_id); + ASSERT_NE(geom, nullptr); + + rtcCommitScene(scene); + + // Check vertices + const Vertex * vertex_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_VERTEX, 0)); + + for (size_t i = 0; i < expected_vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertex_buffer[i], expected_vertices[i]) + } + + // Check triangles + const Triangle * triangle_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_INDEX, 0)); + + for (size_t i = 0; i < expected_vertices.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangle_buffer[i], expected_triangles[i]); + } + + rtcReleaseScene(scene); + rtcReleaseDevice(device); +} + +/** + * @note Test function behavior with vertices set to only zeros. + */ +TEST_F(PrimitiveTest, addToScene_zeros) +{ + const std::vector expected_triangles = {{0, 1, 2}}; + const std::vector expected_vertices = {{0.0f, 0.0f, 0.0f}}; + + primitive_ = + std::make_unique(0.0f, 0.0f, 0.0f, utils::makePose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)); + + RTCDevice device = rtcNewDevice(nullptr); + RTCScene scene = rtcNewScene(device); + + const unsigned int geom_id = primitive_->addToScene(device, scene); + ASSERT_NE(geom_id, RTC_INVALID_GEOMETRY_ID); + + const RTCGeometry geom = rtcGetGeometry(scene, geom_id); + ASSERT_NE(geom, nullptr); + + rtcCommitScene(scene); + + // Check vertices + const Vertex * vertex_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_VERTEX, 0)); + + for (size_t i = 0; i < expected_vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertex_buffer[i], expected_vertices[i]) + } + + // Check triangles + const Triangle * triangle_buffer = + static_cast(rtcGetGeometryBufferData(geom, RTC_BUFFER_TYPE_INDEX, 0)); + + for (size_t i = 0; i < expected_triangles.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangle_buffer[i], expected_triangles[i]); + } + + rtcReleaseScene(scene); + rtcReleaseDevice(device); +} + +/** + * @note Test basic functionality. Test obtaining triangles correctness. + */ +TEST_F(PrimitiveTest, getTriangles) +{ + const std::vector expected_triangles = {{0, 1, 2}, {1, 3, 2}, {4, 6, 5}, {5, 6, 7}, + {0, 4, 1}, {1, 4, 5}, {2, 3, 6}, {3, 7, 6}, + {0, 2, 4}, {2, 6, 4}, {1, 5, 3}, {3, 5, 7}}; + + const auto triangles = primitive_->getTriangles(); + + ASSERT_EQ(triangles.size(), expected_triangles.size()); + for (size_t i = 0; i < triangles.size(); ++i) { + EXPECT_TRIANGLE_EQ(triangles[i], expected_triangles[i]); + } +} + +/** + * @note Test basic functionality. Test obtaining vertexes correctness. + */ +TEST_F(PrimitiveTest, getVertex) +{ + const std::vector expected_vertices = { + {0.0f, 1.0f, -1.0f}, {0.0f, 1.0f, 1.0f}, {0.0f, 3.0f, -1.0f}, {0.0f, 3.0f, 1.0f}, + {2.0f, 1.0f, -1.0f}, {2.0f, 1.0f, 1.0f}, {2.0f, 3.0f, -1.0f}, {2.0f, 3.0f, 1.0f}}; + + const auto vertices = primitive_->getVertex(); + + ASSERT_EQ(vertices.size(), expected_vertices.size()); + for (size_t i = 0; i < vertices.size(); ++i) { + EXPECT_VERTEX_EQ(vertices[i], expected_vertices[i]); + } +} + +/** + * @note Test basic functionality. Test conversion to a convex hull of some concave primitive. + */ +TEST_F(PrimitiveTest, get2DConvexHull_normal) +{ + const std::vector expected_hull = { + utils::makePoint(0.0, 1.0, 0.0), utils::makePoint(0.0, 3.0, 0.0), + utils::makePoint(2.0, 3.0, 0.0), utils::makePoint(2.0, 1.0, 0.0), + utils::makePoint(0.0, 1.0, 0.0)}; + + const auto hull = primitive_->get2DConvexHull(); + + EXPECT_GT(hull.size(), 0); + ASSERT_EQ(hull.size(), expected_hull.size()); + for (size_t i = 0; i < hull.size(); ++i) { + EXPECT_POINT_NEAR(hull[i], expected_hull[i], std::numeric_limits::epsilon()) + } +} + +/** + * @note Test basic functionality. Test conversion to a convex hull of some concave primitive with + * an additional sensor pose transformation - the goal is to test the transformation of convex hull. + */ +TEST_F(PrimitiveTest, get2DConvexHull_withTransform) +{ + const std::vector expected_hull = { + utils::makePoint(-1.0, 0.0, 0.0), utils::makePoint(-1.0, 2.0, 0.0), + utils::makePoint(1.0, 2.0, 0.0), utils::makePoint(1.0, 0.0, 0.0), + utils::makePoint(-1.0, 0.0, 0.0)}; + + const auto sensor_pose = utils::makePose(1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0); + + const auto hull = primitive_->get2DConvexHull(sensor_pose); + + EXPECT_GT(hull.size(), 0); + ASSERT_EQ(hull.size(), expected_hull.size()); + for (size_t i = 0; i < hull.size(); ++i) { + EXPECT_POINT_NEAR(hull[i], expected_hull[i], std::numeric_limits::epsilon()) + } +} + +/** + * @note Test basic functionality. Test min value obtaining in a given axis with a sample primitive. + */ +TEST_F(PrimitiveTest, getMin) +{ + const auto min_x = primitive_->getMin(math::geometry::Axis::X); + + ASSERT_TRUE(min_x.has_value()); + EXPECT_NEAR(min_x.value(), 0.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test min value obtaining in a given axis with a transformation + * with a sample primitive and non trivial sensor pose. + */ +TEST_F(PrimitiveTest, getMin_withTransform) +{ + const auto sensor_pose = utils::makePose(5.0, 2.0, 1.0, 0.0, 0.0, 0.0, 1.0); + + const auto min_x = primitive_->getMin(math::geometry::Axis::X, sensor_pose); + + ASSERT_TRUE(min_x.has_value()); + EXPECT_NEAR(min_x.value(), -5.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test function behavior when vertices is an empty vector - the goal is to get a nullopt. + */ +TEST_F(PrimitiveTest, getMin_empty) +{ + primitive_ = std::make_unique("Unknown", pose_); + + const auto min_x = primitive_->getMin(math::geometry::Axis::X); + + EXPECT_FALSE(min_x.has_value()); +} + +/** + * @note Test basic functionality. Test max value obtaining in a given axis with a sample primitive. + */ +TEST_F(PrimitiveTest, getMax) +{ + const auto max_x = primitive_->getMax(math::geometry::Axis::X); + + ASSERT_TRUE(max_x.has_value()); + EXPECT_NEAR(max_x.value(), 2.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test max value obtaining in a given axis with a transformation + * with a sample primitive and non trivial sensor pose. + */ +TEST_F(PrimitiveTest, getMax_withTransform) +{ + const auto sensor_pose = utils::makePose(6.0, 3.0, 2.0, 0.0, 0.0, 0.0, 1.0); + + const auto max_x = primitive_->getMax(math::geometry::Axis::X, sensor_pose); + + ASSERT_TRUE(max_x.has_value()); + EXPECT_NEAR(max_x.value(), -4.0f, std::numeric_limits::epsilon()); +} + +/** + * @note Test function behavior when vertices is an empty vector - the goal is to get a nullopt. + */ +TEST_F(PrimitiveTest, getMax_empty) +{ + primitive_ = std::make_unique("Unknown", pose_); + + const auto max_x = primitive_->getMax(math::geometry::Axis::X); + + EXPECT_FALSE(max_x.has_value()); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.hpp new file mode 100644 index 00000000000..2343104aa81 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_primitive.hpp @@ -0,0 +1,42 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_SENSOR_SIMULATOR__TEST__TEST_PRIMITIVE_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_PRIMITIVE_HPP_ + +#include + +#include +#include +#include + +#include "../../utils/helper_functions.hpp" + +using namespace simple_sensor_simulator::primitives; +using namespace simple_sensor_simulator; + +class PrimitiveTest : public ::testing::Test +{ +protected: + PrimitiveTest() + : pose_(utils::makePose(1.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0)), + primitive_(std::make_unique(2.0f, 2.0f, 2.0f, pose_)) + { + } + + geometry_msgs::msg::Pose pose_; + std::unique_ptr primitive_; +}; + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_PRIMITIVE_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp index 66da446cec3..c0fe3d57057 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/test_vertex.cpp @@ -19,7 +19,7 @@ #include #include -#include "../expect_eq_macros.hpp" +#include "../../utils/expect_eq_macros.hpp" using namespace simple_sensor_simulator; diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp similarity index 90% rename from simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp rename to simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp index c93387428cc..03346fdc438 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/expect_eq_macros.hpp +++ b/simulation/simple_sensor_simulator/test/src/utils/expect_eq_macros.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #define EXPECT_POINT_EQ(DATA0, DATA1) \ @@ -65,4 +66,9 @@ EXPECT_FLOAT_EQ(VERTEX.y, POINT.y); \ EXPECT_FLOAT_EQ(VERTEX.z, POINT.z); +#define EXPECT_POSITION_NEAR(POSITION0, POSITION1, TOLERANCE) \ + EXPECT_NEAR(POSITION0.x, POSITION1.x, TOLERANCE); \ + EXPECT_NEAR(POSITION0.y, POSITION1.y, TOLERANCE); \ + EXPECT_NEAR(POSITION0.z, POSITION1.z, TOLERANCE); + #endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__EXPECT_EQ_MACROS_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp new file mode 100644 index 00000000000..39adaeee487 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp @@ -0,0 +1,142 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__HELPER_FUNCTIONS_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__HELPER_FUNCTIONS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using EntitySubtype = traffic_simulator_msgs::EntitySubtype; +using EntityType = traffic_simulator_msgs::EntityType; +using EntityStatus = traffic_simulator_msgs::EntityStatus; + +namespace utils +{ + +constexpr auto degToRad(const double deg) -> double { return deg * M_PI / 180.0; } + +inline auto makePoint(const double px, const double py, const double pz) + -> geometry_msgs::msg::Point +{ + return geometry_msgs::build().x(px).y(py).z(pz); +} + +inline auto makePose( + const double px, const double py, const double pz, const double ox, const double oy, + const double oz, const double ow) -> geometry_msgs::msg::Pose +{ + return geometry_msgs::build() + .position(geometry_msgs::build().x(px).y(py).z(pz)) + .orientation(geometry_msgs::build().x(ox).y(oy).z(oz).w(ow)); +} + +inline auto makeDimensions(const double x, const double y, const double z) + -> geometry_msgs::msg::Vector3 +{ + return geometry_msgs::build().x(x).y(y).z(z); +} + +inline auto constructLidarConfiguration( + const std::string & entity, const std::string & architecture_type, + const double lidar_sensor_delay, const double horizontal_resolution) + -> const simulation_api_schema::LidarConfiguration +{ + simulation_api_schema::LidarConfiguration configuration; + configuration.set_horizontal_resolution(horizontal_resolution); + configuration.set_architecture_type(architecture_type); + configuration.set_entity(entity); + configuration.set_lidar_sensor_delay(lidar_sensor_delay); + + configuration.set_scan_duration(0.1); + configuration.add_vertical_angles(degToRad(-15.0)); + configuration.add_vertical_angles(degToRad(-13.0)); + configuration.add_vertical_angles(degToRad(-11.0)); + configuration.add_vertical_angles(degToRad(-9.0)); + configuration.add_vertical_angles(degToRad(-7.0)); + configuration.add_vertical_angles(degToRad(-5.0)); + configuration.add_vertical_angles(degToRad(-3.0)); + configuration.add_vertical_angles(degToRad(-1.0)); + configuration.add_vertical_angles(degToRad(1.0)); + configuration.add_vertical_angles(degToRad(3.0)); + configuration.add_vertical_angles(degToRad(5.0)); + configuration.add_vertical_angles(degToRad(7.0)); + configuration.add_vertical_angles(degToRad(9.0)); + configuration.add_vertical_angles(degToRad(11.0)); + configuration.add_vertical_angles(degToRad(13.0)); + configuration.add_vertical_angles(degToRad(15.0)); + return configuration; +} + +inline auto createEntityStatus( + const std::string & name, const EntityType::Enum type, + const std::optional & subtype, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus +{ + EntityStatus status; + status.set_name(name); + status.mutable_type()->set_type(type); + + if (subtype) { + status.mutable_subtype()->set_value(*subtype); + } + + auto new_pose = status.mutable_pose(); + auto new_position = new_pose->mutable_position(); + new_position->set_x(pose.position.x); + new_position->set_y(pose.position.y); + new_position->set_z(pose.position.z); + + auto new_orientation = new_pose->mutable_orientation(); + new_orientation->set_x(pose.orientation.x); + new_orientation->set_y(pose.orientation.y); + new_orientation->set_z(pose.orientation.z); + new_orientation->set_w(pose.orientation.w); + + auto new_bounding_box = status.mutable_bounding_box(); + auto new_dimensions = new_bounding_box->mutable_dimensions(); + new_dimensions->set_x(dimensions.x); + new_dimensions->set_y(dimensions.y); + new_dimensions->set_z(dimensions.z); + + return status; +} + +inline auto makeEntity( + const std::string & name, const EntityType::Enum type, const EntitySubtype::Enum subtype, + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions) + -> EntityStatus +{ + return createEntityStatus(name, type, subtype, pose, dimensions); +} + +inline auto makeEntity( + const std::string & name, const EntityType::Enum type, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus +{ + return createEntityStatus(name, type, std::nullopt, pose, dimensions); +} + +} // namespace utils + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__HELPER_FUNCTIONS_HPP_ diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 8839262aa35..22a50324def 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,286 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(conversions, behavior_plugin_base): add new line at the end +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge branch 'master' into doc/longitudinal-control +* Merge pull request `#1321 `_ from tier4/feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* feat: Enhance IMU sensor configuration and initialization + - Added frame_id to ImuSensorConfiguration + - Separated noise standard deviations for orientation, twist, and acceleration + - Updated ImuSensorBase and ImuSensor classes for new noise distributions +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* feat(simple_sensor_simulator, imu): add gravity vector, tidy up +* feat(simulator_core, api, zmq): add attachImuSensor, add update imu sensors +* feat(simple_sensor_simulator): add imu_sensor +* Contributors: Dawid Moszynski, Koki Suzuki, Kotaro Yoshimoto, Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp index 1ecd65017cc..72a78f7d456 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp +++ b/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp @@ -67,6 +67,9 @@ class MultiClient auto call(const simulation_api_schema::UpdateEntityStatusRequest &) -> simulation_api_schema::UpdateEntityStatusResponse; + auto call(const simulation_api_schema::AttachImuSensorRequest &) + -> simulation_api_schema::AttachImuSensorResponse; + auto call(const simulation_api_schema::AttachLidarSensorRequest &) -> simulation_api_schema::AttachLidarSensorResponse; diff --git a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp index c9156cbed1e..dea00eb3818 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp +++ b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp @@ -67,6 +67,7 @@ class MultiServer DEFINE_FUNCTION_TYPE(SpawnMiscObjectEntity); DEFINE_FUNCTION_TYPE(DespawnEntity); DEFINE_FUNCTION_TYPE(UpdateEntityStatus); + DEFINE_FUNCTION_TYPE(AttachImuSensor); DEFINE_FUNCTION_TYPE(AttachLidarSensor); DEFINE_FUNCTION_TYPE(AttachDetectionSensor); DEFINE_FUNCTION_TYPE(AttachOccupancyGridSensor); @@ -78,7 +79,7 @@ class MultiServer std::tuple< Initialize, UpdateFrame, SpawnVehicleEntity, SpawnPedestrianEntity, SpawnMiscObjectEntity, - DespawnEntity, UpdateEntityStatus, AttachLidarSensor, AttachDetectionSensor, + DespawnEntity, UpdateEntityStatus, AttachImuSensor, AttachLidarSensor, AttachDetectionSensor, AttachOccupancyGridSensor, UpdateTrafficLights, AttachPseudoTrafficLightDetector, UpdateStepTime> functions_; diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index a12562429a7..1061608f259 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 3.3.0 + 4.3.7 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/simulation_interface/proto/simulation_api_schema.proto b/simulation/simulation_interface/proto/simulation_api_schema.proto index 1fac7fef33e..c123b9e1dc6 100644 --- a/simulation/simulation_interface/proto/simulation_api_schema.proto +++ b/simulation/simulation_interface/proto/simulation_api_schema.proto @@ -33,6 +33,20 @@ message PseudoTrafficLightDetectorConfiguration { string architecture_type = 1; // Autoware architecture type. } +/** + * Parameter configuration of the imu sensor + **/ + message ImuSensorConfiguration { + string entity = 1; // Name of the entity which you want to attach imu. + string frame_id = 2; // Frame ID for the IMU sensor + bool add_gravity = 3; // If true, gravity will be added to the acceleration vector + bool use_seed = 4; // If true, as seed will be used the passed value, if not it will be random + int32 seed = 5; // Seed for random number generator + double noise_standard_deviation_orientation = 6; // The standard deviation for orientation noise (normal distribution, mean = 0.0) + double noise_standard_deviation_twist = 7; // The standard deviation for angular velocity noise (normal distribution, mean = 0.0) + double noise_standard_deviation_acceleration = 8; // The standard deviation for linear acceleration noise (normal distribution, mean = 0.0) +} + /** * Parameter configuration of the lidar sensor **/ @@ -199,6 +213,20 @@ message UpdateEntityStatusResponse { repeated UpdatedEntityStatus status = 2; // List of updated entity status in sensor/dynamics simulator } +/** + * Requests attaching a imu sensor to the target entity. + **/ + message AttachImuSensorRequest { + ImuSensorConfiguration configuration = 1; // Configuration of the imu sensor. +} + +/** + * Requests attaching a imu sensor to the target entity. + **/ + message AttachImuSensorResponse { + Result result = 1; // Result of [AttachImuSensorRequest](#AttachImuSensorRequest) +} + /** * Requests attaching a traffic light detector emulator. **/ @@ -343,6 +371,7 @@ message SimulationRequest { UpdateTrafficLightsRequest update_traffic_lights = 11; AttachPseudoTrafficLightDetectorRequest attach_pseudo_traffic_light_detector = 13; UpdateStepTimeRequest update_step_time = 14; + AttachImuSensorRequest attach_imu_sensor = 15; } } @@ -365,5 +394,6 @@ message SimulationResponse { UpdateTrafficLightsResponse update_traffic_lights = 11; AttachPseudoTrafficLightDetectorResponse attach_pseudo_traffic_light_detector = 13; UpdateStepTimeResponse update_step_time = 14; + AttachImuSensorResponse attach_imu_sensor = 15; } } diff --git a/simulation/simulation_interface/src/zmq_multi_client.cpp b/simulation/simulation_interface/src/zmq_multi_client.cpp index c8993841e3c..a905963f072 100644 --- a/simulation/simulation_interface/src/zmq_multi_client.cpp +++ b/simulation/simulation_interface/src/zmq_multi_client.cpp @@ -146,6 +146,18 @@ auto MultiClient::call(const simulation_api_schema::UpdateEntityStatusRequest & } } +auto MultiClient::call(const simulation_api_schema::AttachImuSensorRequest & request) + -> simulation_api_schema::AttachImuSensorResponse +{ + if (is_running) { + auto simulation_request = simulation_api_schema::SimulationRequest(); + *simulation_request.mutable_attach_imu_sensor() = request; + return call(simulation_request).attach_imu_sensor(); + } else { + return {}; + } +} + auto MultiClient::call(const simulation_api_schema::AttachLidarSensorRequest & request) -> simulation_api_schema::AttachLidarSensorResponse { diff --git a/simulation/simulation_interface/src/zmq_multi_server.cpp b/simulation/simulation_interface/src/zmq_multi_server.cpp index d5c0ee95298..e10bb68cbbd 100644 --- a/simulation/simulation_interface/src/zmq_multi_server.cpp +++ b/simulation/simulation_interface/src/zmq_multi_server.cpp @@ -57,6 +57,10 @@ void MultiServer::poll() *sim_response.mutable_update_entity_status() = std::get(functions_)(proto.update_entity_status()); break; + case simulation_api_schema::SimulationRequest::RequestCase::kAttachImuSensor: + *sim_response.mutable_attach_imu_sensor() = + std::get(functions_)(proto.attach_imu_sensor()); + break; case simulation_api_schema::SimulationRequest::RequestCase::kAttachLidarSensor: *sim_response.mutable_attach_lidar_sensor() = std::get(functions_)(proto.attach_lidar_sensor()); diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index bfc883af579..237b6b06526 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,427 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge pull request `#1369 `_ from tier4/fix/acquire-position-action + Fix/acquire position action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Update `AcquirePositionAction` to request route clearing before applying +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge pull request `#1366 `_ from tier4/RJD-1197/distance + RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* move helper functions to the helper file +* rm bug example +* BUG EXAMPLE +* remove the failing case +* distanceToRightLaneBound tests, cleanup +* distance.cpp bug fixes, distanceToRightLaneBound tests +* remove redundant constructor call +* clean-up; test_fixture +* distance tests skeleton, add intersection map and move standard map to its folder +* longitudinal distance tests +* longitudinal distance tests +* Contributors: Masaya Kataoka, Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ +* Merge pull request `#1387 `_ from tier4/fix/set-flag-invalid-lane-pose + fix(traffic_simulator): set flag valid lanelet pose +* fix(traffic_simulator): set flag valid lanelet pose +* Contributors: Masaya Kataoka, satoshi-ota + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge pull request `#1371 `_ from tier4/RJD-1197/pose_module + Rjd 1197/pose module +* Change names of relativePose tests +* Fix typos in comments +* Merge branch 'master' into RJD-1197/pose_module +* CR requested changes part 2 +* CR requested changes +* Pose module unit tests +* Contributors: Grzegorz Maj, Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ +* Merge pull request `#1367 `_ from tier4/RJD-1197/canonicalized_lanelet_pose + Rjd 1197/canonicalized lanelet pose +* Spelling changes +* Lint changes +* Change assert check from bool to has_value +* Change invalid test cases to more obvious values +* Unit tests review changes +* CanonicalizedLaneletPose unit tests +* Contributors: Grzegorz Maj, Masaya Kataoka + +4.2.0 (2024-09-09) +------------------ +* Merge pull request `#1362 `_ from tier4/feature/ros2-parameter-forwarding +* Cleanup +* Move parameter `use_sim_time` into function `make_parameters` +* Remove data member `traffic_simulator::Configuration::rviz_config_path` +* Add feature to forward parameters prefixed with `autoware.` to Autoware +* Contributors: Kotaro Yoshimoto, yamacir-kit + +4.1.1 (2024-09-03) +------------------ +* Merge pull request `#1207 `_ from tier4/fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(use_sim_time): set default as false +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* use_sim_time used in concealer initialization +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto, Paweł Lech + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge pull request `#1358 `_ from tier4/RJD-1056-remove-npc-logic-started + Remove unused data members: npc_logic_started +* feat(traffic_simulator): apply clang reforamt +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* fix(traffic_simulator): remove unnecessary misc_object tests +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* fix(ego_entity): fix autoware update when not npc_logic_started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Corrections for adapting removed npc logic started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Correct EntityManager::getCurrentAction +* Add else +* Trailing return type +* Use member instead of getter +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Forward isNpcLogicStarted to API and restore TestExecutor +* Add else +* Move implementation to cpp file +* Remove npc_logic_started from API +* Update NPC logic only when it has been started +* Correct style +* Restore previous getCurrentAction behavior +* Remove npc_logic_started from Entities +* Contributors: DMoszynski, Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge pull request `#1356 `_ from tier4/RJD-1056-remove-current-time-step-time + Remove unused data members: current_time step_time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Fix after using pointer to store entity status +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Resolve conflicts with stored time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Trailing return type +* Correct style - add else +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Add const to time argument in behavior +* Add const to time argument +* Use member instead of getter +* Revert to previous macro definition +* Move requestSpeedChange time check responsibility to EgoEntity and simplify +* Correct style +* Remove step_time\_ and current_time\_ data members from EntityManager + Adjust the code so that the time is managed in API only +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge pull request `#1354 `_ from tier4/fix/follow_trajectory + Fix follow trajectory action and timestamp in entity status +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* fix timestamp +* fix timestamp in status_with_trajectory +* Contributors: Masaya Kataoka, Tatsuya Yamasaki + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(distance): use doxygen format + Co-authored-by: Masaya Kataoka +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Fix EgoEntity bug where status time was incremented only when Ego was controlled by simulator + This bug lead to problems in other checks which relied on the correct status time +* Fix EgoEntity error where map pose was unable to be set after scenario start, which should be possible for Ego +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(behavior_tree): use CanonicalizedEntityStatus as shared_ptr inside BT and use ::set +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(ego_entity): fix setMapPose +* ref(traffic_simulator): remove souvenir +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(spell): fix string in api +* feat(traffic_simulator, entity_base): improve setStatus - add passing lanelet_id - use it +* ref(traffic_simulator): use getCanonicalizedStatus, remove getStatus +* ref(traffic_simulator): remove virutla getEntityType, tidy up CanonicalizedEntityStatus getters +* feat(cpp_mock_scenarios): add isPedestrain and isVehicle - use it +* ref(utils, pose): global use pose:: namespace +* fix(traffic_simulator): apply clang reformat +* ref(traffic_simulator): improve CanonicalizedEntityStatus getters - use const ref +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* ref(entity_base): fix retuned type def, fix typo +* doc(developer_guide, traffic_simulator): update doc after review changes, add code notes +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(traffic_simulator): fix helper_functions for tests and misc object tests +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(follow_trajectory_action): fix after merge FTA changes +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* doc(developer_guide, pose utils): adopt lane_pose_calculation doc to canonicalization laneletpose in CanonicalizedEntityStatus +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(traffic_simulator): improve assertions in CanonicalizedEntityStatus::set +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* fix(conversions, behavior_plugin_base): add new line at the end +* ref(traffic_simulator): move toCanonicalizeLaneletPose to CanonicalizedEntityStatus::set, little tidy up +* fix(traffic_simulator): fix behavior_plugin_base +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* ref(traffic_simulator, behavior_tree_plugin): revert unnecessary changes +* ref(traffic_simulator): remove operator= for CanonicalizedEntityStatus, use set and assertions +* fix(traffic_simulator, behavior_tree_plugin): fix returned EntityStatus from bt, fix canonicalize in vehicle_entity and pedestrian_entity +* feat(behavior_tree_plugin, entity_base): move toCanonicalizedEntityPose to traffic_simulator, use EntityStatus as updated_state in BehaviorTree +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* ref(traffic_simulator): remove unnecessary cast to EnrityStatus in EntityManager and MicObjectEntity +* ref(traffic_simulator): remove unecessary casts to EntityStatus in EntityBase +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge pull request `#1348 `_ from tier4/fix/distance-with-lane-change + Fix longitudinal dintance calculation with lane-change in `HdMapUtils::getLongitudinalDistance` +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* chore: add a test for corner case +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* fix: longitudinal calculation with lane-change +* Contributors: Kotaro Yoshimoto, Masaya Kataoka + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge pull request `#1340 `_ from tier4/RJD-1278/traffic_simulator-update + Rjd 1278/traffic simulator update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* spellcheck +* review changes +* test_traffic_light.cpp refactor, sort +* test_traffic_lights_manager.cpp refactor +* remove ros nodes +* test_simulation_clock.cpp refacton +* refactor test_helper.cpp file +* Contributors: Masaya Kataoka, Michał Ciasnocha, robomic + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge pull request `#1335 `_ from tier4/feat/RJD-1283-add-traffic-controller-visualization + feat(traffic_controller, api): add rviz marker for TrafficSink +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* ref(traffic_controller): rename make->appendDebugMarker +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* feat(traffic_controller, api): add rviz marker for TrafficSink +* Contributors: Dawid Moszynski, Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge pull request `#1316 `_ from tier4/relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* refactor: use std::size_t instead of raw size_t + Co-authored-by: Tatsuya Yamasaki +* Merge branch 'master' into relative-clearance-condition +* refactor: use std::find instead of std::find_if +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* fix tests for HdMapUtils::countLaneChanges +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Implement HdMapUtils::countLaneChanges +* Implement HdMapUtils::countLaneChangesAlongRoute +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge branch 'master' into doc/longitudinal-control +* Merge pull request `#1321 `_ from tier4/feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* feat(simulator_core, api, zmq): add attachImuSensor, add update imu sensors +* Contributors: Dawid Moszynski, Koki Suzuki, Kotaro Yoshimoto, Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ +* Merge pull request `#1325 `_ from tier4/feature/interpreter/lidar-configuration + Feature/interpreter/lidar configuration +* Update `MiscObjectEntity` to display with a magenta bounding box +* Contributors: Masaya Kataoka, yamacir-kit + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index c3a502c12d5..1c7879cfe5f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -246,6 +246,10 @@ class API const std::string & name, const geometry_msgs::msg::PoseWithCovarianceStamped & new_pose, const geometry_msgs::msg::PoseStamped & goal_pose) -> void; + auto attachImuSensor( + const std::string &, const simulation_api_schema::ImuSensorConfiguration & configuration) + -> bool; + bool attachPseudoTrafficLightDetector( const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &); diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 3e17d2f5873..155fab255b9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_SIMULATOR__API__CONFIGURATION_HPP_ #include -#include #include #include #include @@ -65,10 +64,6 @@ struct Configuration Pathname scenario_path = ""; - Pathname rviz_config_path = // - ament_index_cpp::get_package_share_directory("traffic_simulator") + - "/config/scenario_simulator_v2.rviz"; - explicit Configuration(const Pathname & map_path) // : map_path(assertMapPath(map_path)), lanelet2_map_file(findLexicographicallyFirstFilenameOf(map_path, ".osm")), diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index 761e5644106..f4162f3e9b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -55,10 +55,10 @@ class BehaviorPluginBase // clang-format off DEFINE_GETTER_SETTER(BehaviorParameter, "behavior_parameter", traffic_simulator_msgs::msg::BehaviorParameter) + DEFINE_GETTER_SETTER(CanonicalizedEntityStatus, "canonicalized_entity_status", std::shared_ptr) DEFINE_GETTER_SETTER(CurrentTime, "current_time", double) DEFINE_GETTER_SETTER(DebugMarker, "debug_marker", std::vector) DEFINE_GETTER_SETTER(DefaultMatchingDistanceForLaneletPoseCalculation, "matching_distance_for_lanelet_pose_calculation", double) - DEFINE_GETTER_SETTER(EntityStatus, "entity_status", std::shared_ptr) DEFINE_GETTER_SETTER(GoalPoses, "goal_poses", std::vector) DEFINE_GETTER_SETTER(HdMapUtils, "hdmap_utils", std::shared_ptr) DEFINE_GETTER_SETTER(LaneChangeParameters, "lane_change_parameters", traffic_simulator::lane_change::Parameter) @@ -72,7 +72,6 @@ class BehaviorPluginBase DEFINE_GETTER_SETTER(StepTime, "step_time", double) DEFINE_GETTER_SETTER(TargetSpeed, "target_speed", std::optional) DEFINE_GETTER_SETTER(TrafficLights, "traffic_lights", std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, "non_canonicalized_updated_status", std::shared_ptr) DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters) DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray) // clang-format on diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index cc33405acdd..c9144204dba 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -45,7 +45,9 @@ class CanonicalizedEntityStatus auto set( const EntityStatus & status, const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) -> void; + auto setAction(const std::string & action) -> void; + auto getActionStatus() const noexcept -> const traffic_simulator_msgs::msg::ActionStatus &; auto getTime() const noexcept -> double; auto setTime(double) -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index 802e4d51459..832dd365a63 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -67,7 +67,7 @@ class EgoEntity : public VehicleEntity auto getCurrentAction() const -> std::string override; - auto getCurrentPose() const -> geometry_msgs::msg::Pose; + auto getCurrentPose() const -> const geometry_msgs::msg::Pose &; auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & override; @@ -143,9 +143,9 @@ class EgoEntity : public VehicleEntity template auto setStatus(Ts &&... xs) -> void { - if (status_.getTime() > 0 && not isControlledBySimulator()) { + if (status_->getTime() > 0 && not isControlledBySimulator()) { THROW_SEMANTIC_ERROR( - "You cannot set entity status to the ego vehicle named ", std::quoted(status_.getName()), + "You cannot set entity status to the ego vehicle named ", std::quoted(status_->getName()), " after starting scenario."); } else { EntityBase::setStatus(std::forward(xs)...); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 2a7d8c7bc90..a68e5361bf3 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -75,19 +75,19 @@ class EntityBase */ \ /* */ auto get##NAME() const noexcept->TYPE { return RETURN_VARIABLE; } - DEFINE_GETTER(BoundingBox, const traffic_simulator_msgs::msg::BoundingBox &, status_.getBoundingBox()) - DEFINE_GETTER(CanonicalizedStatus, const CanonicalizedEntityStatus &, status_) + DEFINE_GETTER(BoundingBox, const traffic_simulator_msgs::msg::BoundingBox &, status_->getBoundingBox()) + DEFINE_GETTER(CanonicalizedStatus, const CanonicalizedEntityStatus &, *status_) DEFINE_GETTER(CanonicalizedStatusBeforeUpdate, const CanonicalizedEntityStatus &, status_before_update_) - DEFINE_GETTER(CurrentAccel, const geometry_msgs::msg::Accel &, status_.getAccel()) - DEFINE_GETTER(CurrentTwist, const geometry_msgs::msg::Twist &, status_.getTwist()) + DEFINE_GETTER(CurrentAccel, const geometry_msgs::msg::Accel &, status_->getAccel()) + DEFINE_GETTER(CurrentTwist, const geometry_msgs::msg::Twist &, status_->getTwist()) DEFINE_GETTER(DynamicConstraints, traffic_simulator_msgs::msg::DynamicConstraints, getBehaviorParameter().dynamic_constraints) - DEFINE_GETTER(EntitySubtype, const traffic_simulator_msgs::msg::EntitySubtype &, status_.getSubtype()) - DEFINE_GETTER(EntityType, const traffic_simulator_msgs::msg::EntityType &, status_.getType()) - DEFINE_GETTER(LinearJerk, double, status_.getLinearJerk()) - DEFINE_GETTER(MapPose, const geometry_msgs::msg::Pose &, status_.getMapPose()) + DEFINE_GETTER(EntitySubtype, const traffic_simulator_msgs::msg::EntitySubtype &, status_->getSubtype()) + DEFINE_GETTER(EntityType, const traffic_simulator_msgs::msg::EntityType &, status_->getType()) + DEFINE_GETTER(LinearJerk, double, status_->getLinearJerk()) + DEFINE_GETTER(MapPose, const geometry_msgs::msg::Pose &, status_->getMapPose()) DEFINE_GETTER(StandStillDuration, double, stand_still_duration_) DEFINE_GETTER(TraveledDistance, double, traveled_distance_) - DEFINE_GETTER(Name, const std::string &, status_.getName()) + DEFINE_GETTER(Name, const std::string &, status_->getName()) // clang-format on #undef DEFINE_GETTER @@ -112,7 +112,7 @@ class EntityBase /* */ auto isInPosition( const CanonicalizedLaneletPose & lanelet_pose, const double tolerance) const -> bool; - /* */ auto isInLanelet() const -> bool { return status_.isInLanelet(); }; + /* */ auto isInLanelet() const -> bool { return status_->isInLanelet(); }; /* */ auto isInLanelet( const lanelet::Id lanelet_id, std::optional tolerance = std::nullopt) const -> bool; @@ -148,7 +148,7 @@ class EntityBase virtual void requestAssignRoute(const std::vector &) = 0; - virtual void requestLaneChange(const lanelet::Id){}; + virtual void requestLaneChange(const lanelet::Id) {} virtual void requestLaneChange(const lane_change::Parameter &){}; @@ -174,7 +174,7 @@ class EntityBase virtual void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool); - virtual void requestClearRoute(); + virtual void requestClearRoute() {} virtual auto isControlledBySimulator() const -> bool; @@ -280,7 +280,7 @@ class EntityBase bool verbose; protected: - CanonicalizedEntityStatus status_; + std::shared_ptr status_; CanonicalizedEntityStatus status_before_update_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index fb2455cd259..21adf1781c7 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -78,6 +78,11 @@ class HdMapUtils const lanelet::Id, const double s, const lanelet::Ids &, const double forward_distance = 20) const -> std::vector; + auto countLaneChanges( + const traffic_simulator_msgs::msg::LaneletPose & from, + const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const + -> std::optional>; + auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids; auto generateMarker() const -> visualization_msgs::msg::MarkerArray; diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp index ac47927668d..75c2a53ea83 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_controller.hpp @@ -46,6 +46,7 @@ class TrafficController const std::function(void)> & get_entity_names_function, const std::function & get_entity_pose_function, const std::function & despawn_function, bool auto_sink = false); + template void addModule(Ts &&... xs) { @@ -53,6 +54,7 @@ class TrafficController modules_.emplace_back(module_ptr); } void execute(const double current_time, const double step_time); + auto makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray; private: void autoSink(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp index 816e2c2faf2..41e52aaf0e4 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_module_base.hpp @@ -26,6 +26,8 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_MODULE_BASE_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_MODULE_BASE_HPP_ +#include + namespace traffic_simulator { namespace traffic @@ -35,6 +37,7 @@ class TrafficModuleBase public: TrafficModuleBase() {} virtual void execute(const double current_time, const double step_time) = 0; + virtual auto appendDebugMarker(visualization_msgs::msg::MarkerArray &) const -> void{}; }; } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp index a6c68b1bfbc..0754ace3f92 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic/traffic_sink.hpp @@ -26,6 +26,8 @@ #ifndef TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_SINK_HPP_ #define TRAFFIC_SIMULATOR__TRAFFIC__TRAFFIC_SINK_HPP_ +#include + #include #include #include @@ -40,13 +42,16 @@ class TrafficSink : public TrafficModuleBase { public: explicit TrafficSink( - double radius, const geometry_msgs::msg::Point & position, + lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names_function, const std::function & get_entity_pose_function, const std::function & despawn_function); + const lanelet::Id lanelet_id; const double radius; const geometry_msgs::msg::Point position; void execute(const double current_time, const double step_time) override; + auto appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const + -> void override; private: const std::function(void)> get_entity_names_function; diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 3e354c32b80..f269ecdc0f9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -33,6 +33,12 @@ auto lateralDistance( double matching_distance, bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; +// Lateral (unit: lanes) +auto countLaneChanges( + const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, + bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) + -> std::optional>; + // Longitudinal auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index c07521057ff..bb19f9d0ee0 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 3.3.0 + 4.3.7 control traffic flow masaya kataoka @@ -51,6 +51,7 @@ ament_cmake_lint_cmake ament_cmake_pep257 ament_cmake_xmllint + kashiwanoha_map ament_cmake diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 0f883154c00..41b6088c5ff 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -117,6 +117,14 @@ auto API::getEntity(const std::string & name) const -> std::shared_ptrgetEntity(name); } +auto API::attachImuSensor( + const std::string &, const simulation_api_schema::ImuSensorConfiguration & configuration) -> bool +{ + simulation_api_schema::AttachImuSensorRequest req; + *req.mutable_configuration() = configuration; + return zeromq_client_.call(req).result().success(); +} + bool API::attachPseudoTrafficLightDetector( const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration) { @@ -270,6 +278,7 @@ bool API::updateFrame() clock_.update(); clock_pub_->publish(clock_.getCurrentRosTimeAsMsg()); debug_marker_pub_->publish(entity_manager_ptr_->makeDebugMarker()); + debug_marker_pub_->publish(traffic_controller_ptr_->makeDebugMarker()); return true; } diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 5051bcc9a56..338de514b19 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -95,6 +95,17 @@ auto CanonicalizedEntityStatus::set( set(status, getLaneletIds(), matching_distance, hdmap_utils_ptr); } +auto CanonicalizedEntityStatus::setAction(const std::string & action) -> void +{ + entity_status_.action_status.current_action = action; +} + +auto CanonicalizedEntityStatus::getActionStatus() const noexcept + -> const traffic_simulator_msgs::msg::ActionStatus & +{ + return entity_status_.action_status; +} + auto CanonicalizedEntityStatus::isInLanelet() const noexcept -> bool { return canonicalized_lanelet_pose_.has_value(); @@ -176,11 +187,6 @@ auto CanonicalizedEntityStatus::setLinearJerk(double linear_jerk) -> void entity_status_.action_status.linear_jerk = linear_jerk; } -auto CanonicalizedEntityStatus::setAction(const std::string & action) -> void -{ - entity_status_.action_status.current_action = action; -} - auto CanonicalizedEntityStatus::getLinearJerk() const noexcept -> double { return entity_status_.action_status.linear_jerk; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index c599f4a0aa3..ba40fc39e71 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -41,23 +41,26 @@ auto EgoEntity::makeFieldOperatorApplication( if (const auto architecture_type = getParameter(node_parameters, "architecture_type", "awf/universe"); architecture_type.find("awf/universe") != std::string::npos) { - std::string rviz_config = getParameter(node_parameters, "rviz_config", ""); + auto parameters = getParameter>(node_parameters, "autoware.", {}); + + // clang-format off + parameters.push_back("map_path:=" + configuration.map_path.string()); + parameters.push_back("lanelet2_map_file:=" + configuration.getLanelet2MapFile()); + parameters.push_back("pointcloud_map_file:=" + configuration.getPointCloudMapFile()); + parameters.push_back("sensor_model:=" + getParameter(node_parameters, "sensor_model")); + parameters.push_back("vehicle_model:=" + getParameter(node_parameters, "vehicle_model")); + parameters.push_back("rviz_config:=" + getParameter(node_parameters, "rviz_config")); + parameters.push_back("scenario_simulation:=true"); + parameters.push_back("use_foa:=false"); + parameters.push_back("perception/enable_traffic_light:=" + std::string(architecture_type >= "awf/universe/20230906" ? "true" : "false")); + parameters.push_back("use_sim_time:=" + std::string(getParameter(node_parameters, "use_sim_time", false) ? "true" : "false")); + // clang-format on + return getParameter(node_parameters, "launch_autoware", true) ? std::make_unique< concealer::FieldOperatorApplicationFor>( getParameter(node_parameters, "autoware_launch_package"), - getParameter(node_parameters, "autoware_launch_file"), - "map_path:=" + configuration.map_path.string(), - "lanelet2_map_file:=" + configuration.getLanelet2MapFile(), - "pointcloud_map_file:=" + configuration.getPointCloudMapFile(), - "sensor_model:=" + getParameter(node_parameters, "sensor_model"), - "vehicle_model:=" + getParameter(node_parameters, "vehicle_model"), - "rviz_config:=" + ((rviz_config == "") - ? configuration.rviz_config_path.string() - : Configuration::Pathname(rviz_config).string()), - "scenario_simulation:=true", "use_foa:=false", - "perception/enable_traffic_light:=" + - std::string((architecture_type >= "awf/universe/20230906") ? "true" : "false")) + getParameter(node_parameters, "autoware_launch_file"), parameters) : std::make_unique< concealer::FieldOperatorApplicationFor>(); } else { @@ -159,7 +162,10 @@ auto EgoEntity::getRouteLanelets(double /*unused horizon*/) -> lanelet::Ids return ids; } -auto EgoEntity::getCurrentPose() const -> geometry_msgs::msg::Pose { return status_.getMapPose(); } +auto EgoEntity::getCurrentPose() const -> const geometry_msgs::msg::Pose & +{ + return status_->getMapPose(); +} auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { @@ -180,16 +186,19 @@ void EgoEntity::onUpdate(double current_time, double step_time) if ( const auto non_canonicalized_updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(status_), *polyline_trajectory_, + static_cast(*status_), *polyline_trajectory_, behavior_parameter_, hdmap_utils_ptr_, step_time, getDefaultMatchingDistanceForLaneletPoseCalculation(), - target_speed_ ? target_speed_.value() : status_.getTwist().linear.x)) { + target_speed_ ? target_speed_.value() : status_->getTwist().linear.x)) { // prefer current lanelet on ss2 side - setStatus(non_canonicalized_updated_status.value(), status_.getLaneletIds()); + setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { is_controlled_by_simulator_ = false; } } + if (not is_controlled_by_simulator_) { + updateEntityStatusTimestamp(current_time + step_time); + } updateStandStillDuration(step_time); updateTraveledDistance(step_time); @@ -301,7 +310,7 @@ auto EgoEntity::setBehaviorParameter( auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void { - if (status_.getTime() > 0.0) { + if (status_->getTime() > 0.0) { THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); } else { target_speed_ = value; @@ -325,14 +334,13 @@ auto EgoEntity::setVelocityLimit(double value) -> void // auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void { - status_.setMapPose(map_pose); - auto entity_status = static_cast(status_); + auto entity_status = static_cast(*status_); + entity_status.pose = map_pose; entity_status.lanelet_pose_valid = false; // prefer current lanelet on Autoware side - const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - status_.getMapPose(), status_.getBoundingBox(), helper::getUniqueValues(getRouteLanelets()), - false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); - setCanonicalizedStatus(CanonicalizedEntityStatus(entity_status, canonicalized_lanelet_pose)); + status_->set( + entity_status, helper::getUniqueValues(getRouteLanelets()), + getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); } } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 121313ed8b6..a64fe00d5d9 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -35,8 +35,8 @@ EntityBase::EntityBase( const std::shared_ptr & hdmap_utils_ptr) : name(name), verbose(true), - status_(entity_status), - status_before_update_(status_), + status_(std::make_shared(entity_status)), + status_before_update_(*status_), hdmap_utils_ptr_(hdmap_utils_ptr) { if (name != static_cast(entity_status).name) { @@ -59,7 +59,7 @@ auto EntityBase::get2DPolygon() const -> std::vector auto EntityBase::getCanonicalizedLaneletPose() const -> std::optional { - return status_.getCanonicalizedLaneletPose(); + return status_->getCanonicalizedLaneletPose(); } auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const @@ -72,7 +72,7 @@ auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const // prefer the current lanelet return pose::toCanonicalizedLaneletPose( - status_.getMapPose(), status_.getBoundingBox(), status_.getLaneletIds(), include_crosswalk, + status_->getMapPose(), status_->getBoundingBox(), status_->getLaneletIds(), include_crosswalk, matching_distance, hdmap_utils_ptr_); } @@ -126,7 +126,7 @@ auto EntityBase::onUpdate(const double /*current_time*/, const double step_time) { job_list_.update(step_time, job::Event::PRE_UPDATE); step_time_ = step_time; - status_before_update_.set(status_); + status_before_update_.set(*status_); speed_planner_ = std::make_unique( step_time, name); @@ -173,7 +173,7 @@ void EntityBase::requestLaneChange( "Source entity does not assigned to lanelet. Please check source entity name : ", name, " exists on lane."); } - reference_lanelet_id = status_.getLaneletId(); + reference_lanelet_id = status_->getLaneletId(); } else { if (other_status_.find(target.entity_name) == other_status_.end()) { THROW_SEMANTIC_ERROR( @@ -251,13 +251,6 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration( } } -void EntityBase::requestClearRoute() -{ - THROW_SEMANTIC_ERROR( - "requestClearRoute is only supported for EgoEntity. The specified Entity is not an EgoEntity. " - "Please check the scenario carefully."); -} - void EntityBase::requestSpeedChangeWithTimeConstraint( const double target_speed, const speed_change::Transition transition, double acceleration_time) { @@ -563,18 +556,18 @@ void EntityBase::setOtherStatus( auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void { - status_.set(status); + status_->set(status); } auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void { - status_.set( + status_->set( status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); } auto EntityBase::setStatus(const EntityStatus & status) -> void { - status_.set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + status_->set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); } auto EntityBase::setStatus( @@ -634,12 +627,12 @@ auto EntityBase::setStatus( auto EntityBase::setLinearVelocity(const double linear_velocity) -> void { - status_.setLinearVelocity(linear_velocity); + status_->setLinearVelocity(linear_velocity); } auto EntityBase::setLinearAcceleration(const double linear_acceleration) -> void { - status_.setLinearAcceleration(linear_acceleration); + status_->setLinearAcceleration(linear_acceleration); } void EntityBase::setTrafficLights( @@ -650,20 +643,20 @@ void EntityBase::setTrafficLights( auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void { - status_.setTwist(twist); + status_->setTwist(twist); } auto EntityBase::setAcceleration(const geometry_msgs::msg::Accel & accel) -> void { - status_.setAccel(accel); + status_->setAccel(accel); } auto EntityBase::setLinearJerk(const double linear_jerk) -> void { - status_.setLinearJerk(linear_jerk); + status_->setLinearJerk(linear_jerk); } -auto EntityBase::setAction(const std::string & action) -> void { status_.setAction(action); } +auto EntityBase::setAction(const std::string & action) -> void { status_->setAction(action); } auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void { @@ -715,14 +708,14 @@ void EntityBase::activateOutOfRangeJob( void EntityBase::stopAtCurrentPosition() { - status_.setTwist(geometry_msgs::msg::Twist()); - status_.setAccel(geometry_msgs::msg::Accel()); - status_.setLinearJerk(0.0); + status_->setTwist(geometry_msgs::msg::Twist()); + status_->setAccel(geometry_msgs::msg::Accel()); + status_->setLinearJerk(0.0); } void EntityBase::updateEntityStatusTimestamp(const double current_time) { - status_.setTime(current_time); + status_->setTime(current_time); } auto EntityBase::updateStandStillDuration(const double step_time) -> double diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index 6a8e1c154ea..9198aa4af61 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -332,6 +332,7 @@ void EntityManager::update(const double current_time, const double step_time) status_with_trajectory.obstacle_find = false; } status_with_trajectory.status = static_cast(status); + status_with_trajectory.status.time = current_time + step_time; status_with_trajectory.name = name; status_with_trajectory.time = current_time + step_time; status_array_msg.data.emplace_back(status_with_trajectory); diff --git a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp index 72665a586d2..1f05561e4ea 100644 --- a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp +++ b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp @@ -33,12 +33,12 @@ auto MiscObjectEntity::onUpdate(const double /*current_time*/, const double step setLinearJerk(0.0); setAction("static"); updateStandStillDuration(step_time); - status_before_update_.set(status_); + status_before_update_.set(*status_); } auto MiscObjectEntity::getCurrentAction() const -> std::string { - return static_cast(status_).action_status.current_action; + return static_cast(*status_).action_status.current_action; } auto MiscObjectEntity::getBehaviorParameter() const diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 3e009b28bee..b76867a869d 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -71,7 +71,7 @@ void PedestrianEntity::requestAssignRoute(const std::vectorgetBoundingBox(), true, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { @@ -106,7 +106,7 @@ auto PedestrianEntity::getDefaultDynamicConstraints() const auto PedestrianEntity::getRouteLanelets(double horizon) -> lanelet::Ids { - if (const auto canonicalized_lanelet_pose = status_.getCanonicalizedLaneletPose()) { + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon); } else { return {}; @@ -136,7 +136,7 @@ void PedestrianEntity::requestWalkStraight() void PedestrianEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.isInLanelet()) { + if (status_->isInLanelet()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); @@ -147,7 +147,7 @@ void PedestrianEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & m behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), true, + map_pose, status_->getBoundingBox(), true, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { @@ -249,16 +249,13 @@ void PedestrianEntity::setDecelerationRateLimit(double deceleration_rate) auto PedestrianEntity::onUpdate(const double current_time, const double step_time) -> void { EntityBase::onUpdate(current_time, step_time); - behavior_plugin_ptr_->setOtherEntityStatus(other_status_); - behavior_plugin_ptr_->setEntityStatus( - std::make_shared(status_)); + behavior_plugin_ptr_->setCanonicalizedEntityStatus(status_); behavior_plugin_ptr_->setTargetSpeed(target_speed_); behavior_plugin_ptr_->setRouteLanelets(getRouteLanelets()); + /// @note CanonicalizedEntityStatus is updated here, it is not skipped even if isAtEndOfLanelets return true behavior_plugin_ptr_->update(current_time, step_time); - setStatus(*behavior_plugin_ptr_->getUpdatedStatus()); - /// @note setStatus() is not skipped even if isAtEndOfLanelets return true - if (const auto canonicalized_lanelet_pose = status_.getCanonicalizedLaneletPose()) { + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { stopAtCurrentPosition(); updateStandStillDuration(step_time); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index ae63888e9d5..3705f00da13 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -101,7 +101,7 @@ auto VehicleEntity::getObstacle() -> std::optional lanelet::Ids { - if (const auto canonicalized_lanelet_pose = status_.getCanonicalizedLaneletPose()) { + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { return route_planner_.getRouteLanelets(canonicalized_lanelet_pose.value(), horizon); } else { return {}; @@ -113,7 +113,7 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin try { return behavior_plugin_ptr_->getWaypoints(); } catch (const std::runtime_error & e) { - if (not status_.isInLanelet()) { + if (not status_->isInLanelet()) { THROW_SIMULATION_ERROR( "Failed to calculate waypoints in NPC logics, please check Entity : ", name, " is in a lane coordinate."); @@ -128,7 +128,7 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time) EntityBase::onUpdate(current_time, step_time); behavior_plugin_ptr_->setOtherEntityStatus(other_status_); - behavior_plugin_ptr_->setEntityStatus(std::make_unique(status_)); + behavior_plugin_ptr_->setCanonicalizedEntityStatus(status_); behavior_plugin_ptr_->setTargetSpeed(target_speed_); auto route_lanelets = getRouteLanelets(); behavior_plugin_ptr_->setRouteLanelets(route_lanelets); @@ -145,10 +145,9 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time) } } behavior_plugin_ptr_->setReferenceTrajectory(spline_); + /// @note CanonicalizedEntityStatus is updated here, it is not skipped even if isAtEndOfLanelets return true behavior_plugin_ptr_->update(current_time, step_time); - setStatus(*behavior_plugin_ptr_->getUpdatedStatus()); - /// @note setStatus() is not skipped even if isAtEndOfLanelets return true - if (const auto canonicalized_lanelet_pose = status_.getCanonicalizedLaneletPose()) { + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { stopAtCurrentPosition(); updateStandStillDuration(step_time); @@ -165,7 +164,7 @@ auto VehicleEntity::onUpdate(const double current_time, const double step_time) void VehicleEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.isInLanelet()) { + if (status_->isInLanelet()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); @@ -176,7 +175,7 @@ void VehicleEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_ behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), false, + map_pose, status_->getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { @@ -204,7 +203,7 @@ void VehicleEntity::requestAssignRoute(const std::vectorgetBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { @@ -223,7 +222,7 @@ auto VehicleEntity::requestFollowTrajectory( for (const auto & vertex : parameter->shape.vertices) { if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - vertex.position, status_.getBoundingBox(), false, + vertex.position, status_->getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { waypoints.emplace_back(canonicalized_lanelet_pose.value()); } else { diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 36842da9b7a..821a2fd0acd 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -214,6 +214,37 @@ auto HdMapUtils::canonicalizeLaneletPose( return {canonicalized, std::nullopt}; } +auto HdMapUtils::countLaneChanges( + const traffic_simulator_msgs::msg::LaneletPose & from, + const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const + -> std::optional> +{ + const auto route = getRoute(from.lanelet_id, to.lanelet_id, allow_lane_change); + if (route.empty()) { + return std::nullopt; + } else { + std::pair lane_changes{0, 0}; + for (std::size_t i = 1; i < route.size(); ++i) { + const auto & previous = route[i - 1]; + const auto & current = route[i]; + + if (auto followings = getNextLaneletIds(previous); + std::find(followings.begin(), followings.end(), current) == followings.end()) { + traffic_simulator_msgs::msg::EntityType type; + type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + if (auto lefts = getLeftLaneletIds(previous, type); + std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { + lane_changes.first++; + } else if (auto rights = getRightLaneletIds(previous, type); + std::find(rights.begin(), rights.end(), current) != rights.end()) { + lane_changes.second++; + } + } + } + return lane_changes; + } +} + auto HdMapUtils::getLaneletIds() const -> lanelet::Ids { lanelet::Ids ids; @@ -1560,9 +1591,9 @@ auto HdMapUtils::getLongitudinalDistance( /// @note "first lanelet before the lane change" case if (route[i] == from.lanelet_id) { - distance += getLaneletLength(route[i + 1]) - from.s; + distance -= from.s; if (route[i + 1] == to.lanelet_id) { - distance -= getLaneletLength(route[i + 1]) - to.s; + distance += to.s; return distance; } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index a5bdbdec8cd..cd8ab6d5a37 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -61,7 +61,8 @@ void TrafficController::autoSink() lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_); const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_); addModule( - 1, pose.position, get_entity_names_function, get_entity_pose_function, despawn_function); + lanelet_id, 1, pose.position, get_entity_names_function, get_entity_pose_function, + despawn_function); } } } @@ -72,5 +73,17 @@ void TrafficController::execute(const double current_time, const double step_tim module->execute(current_time, step_time); } } + +auto TrafficController::makeDebugMarker() const -> const visualization_msgs::msg::MarkerArray +{ + static const auto marker_array = [&]() { + visualization_msgs::msg::MarkerArray marker_array; + for (size_t i = 0; i < modules_.size(); ++i) { + modules_[i]->appendDebugMarker(marker_array); + } + return marker_array; + }(); + return marker_array; +} } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp index c98fcdf95bd..b85663f17d6 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_sink.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_sink.cpp @@ -23,8 +23,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include +#include #include #include #include @@ -36,11 +38,12 @@ namespace traffic_simulator namespace traffic { TrafficSink::TrafficSink( - double radius, const geometry_msgs::msg::Point & position, + lanelet::Id lanelet_id, double radius, const geometry_msgs::msg::Point & position, const std::function(void)> & get_entity_names_function, const std::function & get_entity_pose_function, const std::function & despawn_function) : TrafficModuleBase(), + lanelet_id(lanelet_id), radius(radius), position(position), get_entity_names_function(get_entity_names_function), @@ -60,5 +63,34 @@ void TrafficSink::execute( } } } + +auto TrafficSink::appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) const + -> void +{ + const auto lanelet_text = std::to_string(lanelet_id); + visualization_msgs::msg::Marker traffic_sink_marker; + traffic_sink_marker.header.frame_id = "map"; + traffic_sink_marker.ns = "traffic_controller/traffic_sink/" + lanelet_text; + traffic_sink_marker.id = 0; + traffic_sink_marker.action = traffic_sink_marker.ADD; + traffic_sink_marker.type = 3; // cylinder + traffic_sink_marker.pose = + geometry_msgs::build().position(position).orientation( + geometry_msgs::build().x(0).y(0).z(0).w(1)); + traffic_sink_marker.color = color_names::makeColorMsg("firebrick", 0.99); + traffic_sink_marker.scale.x = radius * 2; + traffic_sink_marker.scale.y = radius * 2; + traffic_sink_marker.scale.z = 1.0; + marker_array.markers.emplace_back(traffic_sink_marker); + + visualization_msgs::msg::Marker text_marker; + text_marker = traffic_sink_marker; + text_marker.id = 1; + text_marker.type = 9; //text + text_marker.text = lanelet_text; + text_marker.color = color_names::makeColorMsg("white", 0.99); + text_marker.scale.z = 0.6; + marker_array.markers.emplace_back(text_marker); +} } // namespace traffic } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index bf235e9a85c..af12722401e 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -45,10 +45,16 @@ auto lateralDistance( } } -/* - See doc: - https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md -*/ +auto countLaneChanges( + const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, + bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) + -> std::optional> +{ + return hdmap_utils_ptr->countLaneChanges( + static_cast(from), static_cast(to), allow_lane_change); +} + +/// @sa https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, bool include_adjacent_lanelet, bool include_opposite_direction, bool allow_lane_change, @@ -208,6 +214,9 @@ auto distanceToLeftLaneBound( const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, const std::shared_ptr & hdmap_utils_ptr) -> double { + if (lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR("Failing to calculate distanceToLeftLaneBound given an empty vector."); + } std::vector distances; std::transform( lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) { @@ -239,10 +248,13 @@ auto distanceToRightLaneBound( const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & lanelet_ids, const std::shared_ptr & hdmap_utils_ptr) -> double { + if (lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR("Failing to calculate distanceToRightLaneBound for given empty vector."); + } std::vector distances; std::transform( lanelet_ids.begin(), lanelet_ids.end(), std::back_inserter(distances), [&](auto lanelet_id) { - return distanceToLeftLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); + return distanceToRightLaneBound(map_pose, bounding_box, lanelet_id, hdmap_utils_ptr); }); return *std::min_element(distances.begin(), distances.end()); } diff --git a/simulation/traffic_simulator/src/visualization/visualization_component.cpp b/simulation/traffic_simulator/src/visualization/visualization_component.cpp index 6cea709f9d8..f1510972d8e 100644 --- a/simulation/traffic_simulator/src/visualization/visualization_component.cpp +++ b/simulation/traffic_simulator/src/visualization/visualization_component.cpp @@ -124,18 +124,20 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke constexpr auto default_quaternion = rosidl_runtime_cpp::MessageInitialization::DEFAULTS_ONLY; auto ret = visualization_msgs::msg::MarkerArray(); auto stamp = get_clock()->now(); - std_msgs::msg::ColorRGBA color; - switch (status.type.type) { - case status.type.EGO: - color = color_names::makeColorMsg("limegreen", 0.99); - break; - case status.type.PEDESTRIAN: - color = color_names::makeColorMsg("orange", 0.99); - break; - case status.type.VEHICLE: - color = color_names::makeColorMsg("lightskyblue", 0.99); - break; - } + + const auto color = [&]() { + switch (status.type.type) { + case status.type.EGO: + return color_names::makeColorMsg("limegreen", 0.99); + case status.type.PEDESTRIAN: + return color_names::makeColorMsg("orange", 0.99); + case status.type.VEHICLE: + return color_names::makeColorMsg("lightskyblue", 0.99); + default: + case status.type.MISC_OBJECT: + return color_names::makeColorMsg("magenta", 0.99); + } + }(); if (goal_pose.size() != 0) { goal_pose_max_size = std::max(goal_pose_max_size, int(goal_pose.size())); diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 713c8b40678..08f9f50199c 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -1,7 +1,9 @@ -add_subdirectory(src/traffic_lights) -add_subdirectory(src/helper) -add_subdirectory(src/entity) add_subdirectory(src/behavior) +add_subdirectory(src/data_type) +add_subdirectory(src/entity) +add_subdirectory(src/hdmap_utils) +add_subdirectory(src/helper) add_subdirectory(src/job) add_subdirectory(src/simulation_clock) -add_subdirectory(src/hdmap_utils) +add_subdirectory(src/traffic_lights) +add_subdirectory(src/utils) diff --git a/simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm b/simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm new file mode 100644 index 00000000000..3a239c64159 --- /dev/null +++ b/simulation/traffic_simulator/test/map/intersection/lanelet2_map.osm @@ -0,0 +1,15432 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulation/traffic_simulator/test/map/lanelet2_map.osm b/simulation/traffic_simulator/test/map/standard_map/lanelet2_map.osm similarity index 100% rename from simulation/traffic_simulator/test/map/lanelet2_map.osm rename to simulation/traffic_simulator/test/map/standard_map/lanelet2_map.osm diff --git a/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt b/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt new file mode 100644 index 00000000000..6bbebabc879 --- /dev/null +++ b/simulation/traffic_simulator/test/src/data_type/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_lanelet_pose test_lanelet_pose.cpp) +target_link_libraries(test_lanelet_pose traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp new file mode 100644 index 00000000000..24d74f063ce --- /dev/null +++ b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp @@ -0,0 +1,384 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "../helper_functions.hpp" + +using CanonicalizedLaneletPose = traffic_simulator::lanelet_pose::CanonicalizedLaneletPose; + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +class CanonicalizedLaneletPoseTest : public testing::Test +{ +protected: + CanonicalizedLaneletPoseTest() : hdmap_utils(makeHdMapUtilsSharedPointer()) {} + + std::shared_ptr hdmap_utils; +}; + +/** + * @note Test constructor behavior with route_lanelets argument present when canonicalization function fails. + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute_invalid) +{ + EXPECT_THROW( + CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), lanelet::Ids{}, + hdmap_utils), + std::runtime_error); +} + +/** + * @note Test constructor behavior with route_lanelets argument present when canonicalization function succeeds + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute) +{ + std::shared_ptr pose; + EXPECT_NO_THROW( + pose = std::make_shared( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), lanelet::Ids{120659}, + hdmap_utils)); + EXPECT_LANELET_POSE_EQ( + static_cast(*pose), + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); +} + +/** + * @note Test constructor behavior with route_lanelets argument absent when canonicalization function fails + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute_invalid) +{ + EXPECT_THROW( + CanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), hdmap_utils), + std::runtime_error); +} + +/** + * @note Test constructor behavior with route_lanelets argument absent when canonicalization function succeeds + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute) +{ + std::shared_ptr pose; + EXPECT_NO_THROW( + pose = std::make_shared( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils)); + EXPECT_LANELET_POSE_EQ( + static_cast(*pose), + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); +} + +/** + * @note Test copy constructor behavior + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_copyConstructor) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_copy(pose); + EXPECT_LANELET_POSE_EQ( + static_cast(pose), + static_cast(CanonicalizedLaneletPose(pose))); +} + +/** + * @note Test move constructor behavior + */ +TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_moveConstructor) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose2(pose); + const CanonicalizedLaneletPose pose_move = std::move(pose2); + EXPECT_LANELET_POSE_EQ( + static_cast(pose), + static_cast(pose_move)); +} + +/** + * @note Test copy assignment operator behavior + */ +TEST_F(CanonicalizedLaneletPoseTest, copyAssignment) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + CanonicalizedLaneletPose pose_assign( + traffic_simulator::helper::constructLaneletPose(34468, 0.0, 0.0), hdmap_utils); + + pose_assign = pose; + + EXPECT_LANELET_POSE_EQ( + static_cast(pose), + static_cast(pose_assign)); +} + +/** + * @note Test conversion operator behavior using static_cast + */ +TEST_F(CanonicalizedLaneletPoseTest, conversionLaneletPose) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const traffic_simulator::LaneletPose pose_casted = + static_cast(pose); + + EXPECT_EQ(pose_casted.lanelet_id, 120659); + EXPECT_DOUBLE_EQ(pose_casted.s, 0.0); + EXPECT_DOUBLE_EQ(pose_casted.offset, 0.0); + EXPECT_VECTOR3_EQ(pose_casted.rpy, geometry_msgs::msg::Vector3()); +} + +/** + * @note Test conversion operator behavior using static_cast + */ +TEST_F(CanonicalizedLaneletPoseTest, conversionPose) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + + const geometry_msgs::msg::Pose pose1 = + makePose(makePoint(3822.3815, 73784.9618, -1.761), makeQuaternionFromYaw(2.060578777273)); + + EXPECT_POSE_NEAR(static_cast(pose), pose1, 0.01); +} + +/** + * @note Test function behavior when alternative poses are present + */ +TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_true) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, -10.0, 0.0), hdmap_utils); + + EXPECT_TRUE(pose.hasAlternativeLaneletPose()); +} + +/** + * @note Test function behavior when alternative poses are absent + */ +TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_false) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 10.0, 0.0), hdmap_utils); + + EXPECT_FALSE(pose.hasAlternativeLaneletPose()); +} + +/** + * @note Test function behavior when there are no lanelet_poses + */ +TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_empty) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 20.0, 0.0), hdmap_utils); + const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); + const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); + + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + + ASSERT_TRUE(result1.has_value()); + ASSERT_TRUE(result2.has_value()); + + EXPECT_EQ(result1.value().lanelet_id, 120659); + EXPECT_EQ(result2.value().lanelet_id, 120659); +} + +/** + * @note Test function behavior when there is only one lanelet_pose + */ +TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_single) +{ + CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(34666, -20.0, 0.0), hdmap_utils); + const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); + const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); + + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + + ASSERT_TRUE(result1.has_value()); + ASSERT_TRUE(result2.has_value()); + + EXPECT_EQ(result1.value().lanelet_id, 34603); + EXPECT_EQ(result2.value().lanelet_id, 34603); +} + +/** + * @note Test function behavior when there are multiple lanelet_poses + */ +TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_multiple) +{ + CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, -20.0, 0.0), hdmap_utils); + const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); + const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); + + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + + ASSERT_TRUE(result1.has_value()); + ASSERT_TRUE(result2.has_value()); + + EXPECT_EQ(result1.value().lanelet_id, 34603); + EXPECT_EQ(result2.value().lanelet_id, 34579); +} + +/** + * @note Test accessor function + */ +TEST(CanonicalizedLaneletPose, getConsiderPoseByRoadSlope) +{ + EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), false); +} + +/** + * @note Test accessor function + */ +TEST(CanonicalizedLaneletPose, setConsiderPoseByRoadSlope) +{ + EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), false); + CanonicalizedLaneletPose::setConsiderPoseByRoadSlope(true); + EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), true); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorLessEqual) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_TRUE(pose_less <= pose); + EXPECT_TRUE(pose_equal <= pose); + EXPECT_FALSE(pose_greater <= pose); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorLess) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_TRUE(pose_less < pose); + EXPECT_FALSE(pose_equal < pose); + EXPECT_FALSE(pose_greater < pose); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorGreaterEqual) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_FALSE(pose_less >= pose); + EXPECT_TRUE(pose_equal >= pose); + EXPECT_TRUE(pose_greater >= pose); +} + +/** + * @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, operatorGreater) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_equal( + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_less( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose_greater( + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + + EXPECT_FALSE(pose_less > pose); + EXPECT_FALSE(pose_equal > pose); + EXPECT_TRUE(pose_greater > pose); +} + +/** + * @note Test function behavior when provided two poses occupying the same lanelet_id + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_same) +{ + const CanonicalizedLaneletPose pose1( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + const CanonicalizedLaneletPose pose2( + traffic_simulator::helper::constructLaneletPose(120659, 1.0, 0.0), hdmap_utils); + + EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose1, pose2)); +} + +/** + * @note Test function behavior when provided two poses occupying different lanelet_ids + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_different) +{ + const CanonicalizedLaneletPose pose1( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + CanonicalizedLaneletPose pose2( + traffic_simulator::helper::constructLaneletPose(34606, 1.0, 0.0), hdmap_utils); + + EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose1, pose2)); +} + +/** + * @note Test function behavior when provided with a pose having lanelet_id equal to the lanelet_id argument + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_same) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + + EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose, 120659)); +} + +/** + * @note Test function behavior when provided with a pose having lanelet_id different to the lanelet_id argument + */ +TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_different) +{ + const CanonicalizedLaneletPose pose( + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + + EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose, 34606)); +} diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index 1ac7370e66e..26b5efaa132 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -34,12 +34,13 @@ class HdMapUtilsTest_StandardMap : public testing::Test { protected: HdMapUtilsTest_StandardMap() - : hdmap_utils(hdmap_utils::HdMapUtils( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + : hdmap_utils( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) - .altitude(0.0))) + .altitude(0.0)) { } @@ -49,13 +50,13 @@ class HdMapUtilsTest_WithRoadShoulderMap : public testing::Test { protected: HdMapUtilsTest_WithRoadShoulderMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/with_road_shoulder/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) - .altitude(0.0))) + .altitude(0.0)) { } @@ -65,13 +66,13 @@ class HdMapUtilsTest_EmptyMap : public testing::Test { protected: HdMapUtilsTest_EmptyMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/empty/lanelet2_map.osm", geographic_msgs::build() .latitude(0.0) .longitude(0.0) - .altitude(0.0))) + .altitude(0.0)) { } @@ -81,13 +82,13 @@ class HdMapUtilsTest_FourTrackHighwayMap : public testing::Test { protected: HdMapUtilsTest_FourTrackHighwayMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/four_track_highway/lanelet2_map.osm", geographic_msgs::build() .latitude(35.22312494055522) .longitude(138.8024583466017) - .altitude(0.0))) + .altitude(0.0)) { } @@ -97,18 +98,34 @@ class HdMapUtilsTest_CrossroadsWithStoplinesMap : public testing::Test { protected: HdMapUtilsTest_CrossroadsWithStoplinesMap() - : hdmap_utils(hdmap_utils::HdMapUtils( + : hdmap_utils( ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/crossroads_with_stoplines/lanelet2_map.osm", geographic_msgs::build() .latitude(35.23808753540768) .longitude(139.9009591876285) - .altitude(0.0))) + .altitude(0.0)) + { + } + + hdmap_utils::HdMapUtils hdmap_utils; +}; +class HdMapUtilsTest_KashiwanohaMap : public testing::Test +{ +protected: + HdMapUtilsTest_KashiwanohaMap() + : hdmap_utils( + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(0.0) + .longitude(0.0) + .altitude(0.0)) { } hdmap_utils::HdMapUtils hdmap_utils; }; + /** * @note Test basic functionality. * Test initialization correctness with a correct path to a lanelet map. @@ -117,7 +134,8 @@ TEST(HdMapUtils, Construct) { ASSERT_NO_THROW( auto hdmap_utils = hdmap_utils::HdMapUtils( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.61836750154) .longitude(139.78066608243) @@ -447,6 +465,46 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAll) EXPECT_EQ(canonicalized_lanelet_poses[0].s, non_canonicalized_lanelet_s); } +/** + * @note Testcase for countLaneChanges() function + */ +TEST_F(HdMapUtilsTest_FourTrackHighwayMap, CountLaneChangesAlongRoute) +{ + using traffic_simulator::helper::constructLaneletPose; + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(3002175, 0), true), + std::make_pair(1, 0)); + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(3002182, 0), true), + std::make_pair(1, 0)); + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(199, 0), true), + std::make_pair(1, 0)); + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(3002176, 0), true), + std::make_pair(0, 0)); + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(200, 0), true), + std::make_pair(0, 0)); + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(201, 0), true), + std::make_pair(0, 1)); + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(202, 0), true), + std::make_pair(0, 2)); + EXPECT_EQ( + hdmap_utils.countLaneChanges( + constructLaneletPose(3002176, 0), constructLaneletPose(206, 0), true), + std::make_pair(0, 2)); +} + /** * @note Test basic functionality. * Test filtering correctness with some lanelet ids and a valid subtype name. @@ -1984,6 +2042,19 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLane hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false).has_value()); } +/** + * @note Test for the corner-case fixed in https://github.com/tier4/scenario_simulator_v2/pull/1348. + */ +TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348) +{ + auto pose_from = traffic_simulator::helper::constructLaneletPose(34468, 10.0); + auto pose_to = traffic_simulator::helper::constructLaneletPose(34795, 5.0); + + EXPECT_NO_THROW(EXPECT_DOUBLE_EQ( + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true).value(), + 54.18867466433655977198213804513216018676757812500000)); +} + /** * @note Test basic functionality. * Test obtaining stop line ids correctness with a route that has no stop lines. diff --git a/simulation/traffic_simulator/test/src/helper/test_helper.cpp b/simulation/traffic_simulator/test/src/helper/test_helper.cpp index 06c2f02e24a..4ddfdfa28e1 100644 --- a/simulation/traffic_simulator/test/src/helper/test_helper.cpp +++ b/simulation/traffic_simulator/test/src/helper/test_helper.cpp @@ -14,78 +14,105 @@ #include +#include #include #include #include #include "../expect_eq_macros.hpp" -TEST(HELPER, RPY) +/** + * @note Test basic functionality. Test construction correctness of an action status. + */ +TEST(helper, constructActionStatus) { - const auto rpy = traffic_simulator::helper::constructRPY(1, 2, 1); - geometry_msgs::msg::Vector3 vec; - vec.x = 1.0; - vec.y = 2.0; - vec.z = 1.0; - EXPECT_VECTOR3_EQ(rpy, vec); -} + traffic_simulator_msgs::msg::ActionStatus actual_action_status; + actual_action_status.twist.linear.x = 2.0; + actual_action_status.twist.angular.z = 3.0; + actual_action_status.accel.linear.x = 5.0; + actual_action_status.accel.angular.z = 7.0; -TEST(HELPER, QUATERNION) -{ - const auto rpy = - traffic_simulator::helper::constructRPYfromQuaternion(geometry_msgs::msg::Quaternion()); - EXPECT_VECTOR3_EQ(rpy, geometry_msgs::msg::Vector3()); + const auto result_action_status = + traffic_simulator::helper::constructActionStatus(2.0, 3.0, 5.0, 7.0); + + EXPECT_ACTION_STATUS_EQ(result_action_status, actual_action_status); } -TEST(HELPER, POSE) +/** + * @note Test basic functionality. Test construction correctness of a lanelet pose. + */ +TEST(helper, constructLaneletPose) { - const auto pose = traffic_simulator::helper::constructPose(1, 1, 1, 0, 0, 0); - geometry_msgs::msg::Pose expect_pose; - expect_pose.position.x = 1; - expect_pose.position.y = 1; - expect_pose.position.z = 1; - EXPECT_POSE_EQ(pose, expect_pose); + const auto actual_lanelet_pose = + traffic_simulator_msgs::build() + .lanelet_id(11LL) + .s(13.0) + .offset(17.0) + .rpy(geometry_msgs::build().x(19.0).y(23.0).z(29.0)); + + const auto result_lanelet_pose = + traffic_simulator::helper::constructLaneletPose(11LL, 13.0, 17.0, 19.0, 23.0, 29.0); + + std::stringstream ss; + ss << result_lanelet_pose; + EXPECT_LANELET_POSE_EQ(result_lanelet_pose, actual_lanelet_pose); + EXPECT_STREQ(ss.str().c_str(), "lanelet id : 11\ns : 13"); } -TEST(HELPER, LANELET_POSE) +/** + * @note Test basic functionality. Test construction correctness of RPY vector. + */ +TEST(helper, constructRPY) { - const auto lanelet_pose = traffic_simulator::helper::constructLaneletPose(5, 10, 2, 0, 0, 0); - traffic_simulator::LaneletPose expected_pose; - expected_pose.lanelet_id = 5; - expected_pose.s = 10.0; - expected_pose.offset = 2.0; - expected_pose.rpy.x = 0; - expected_pose.rpy.y = 0; - expected_pose.rpy.z = 0; - EXPECT_LANELET_POSE_EQ(lanelet_pose, expected_pose); - std::stringstream ss; - ss << lanelet_pose; - EXPECT_STREQ(ss.str().c_str(), "lanelet id : 5\ns : 10"); + const auto vec = geometry_msgs::build().x(31.0).y(37.0).z(41.0); + const auto rpy = traffic_simulator::helper::constructRPY(31.0, 37.0, 41.0); + EXPECT_VECTOR3_EQ(rpy, vec); } -TEST(HELPER, ACTION_STATUS) +/** + * @note Test basic functionality. Test construction correctness of a quaternion. + */ +TEST(helper, constructRPYfromQuaternion) { - const auto action_status = traffic_simulator::helper::constructActionStatus(3, 4, 5, 1); - traffic_simulator_msgs::msg::ActionStatus expect_action_status; - expect_action_status.twist.linear.x = 3; - expect_action_status.twist.angular.z = 4; - expect_action_status.accel.linear.x = 5; - expect_action_status.accel.angular.z = 1; - EXPECT_ACTION_STATUS_EQ(action_status, expect_action_status); + { + const auto default_vec = geometry_msgs::msg::Vector3(); + const auto default_rpy = + traffic_simulator::helper::constructRPYfromQuaternion(geometry_msgs::msg::Quaternion()); + EXPECT_VECTOR3_EQ(default_rpy, default_vec); + } + { + const auto vec = geometry_msgs::build() + .x(-M_PI / 3.0) + .y(-M_PI / 6.0) + .z(M_PI / 2.0); + const auto rpy = traffic_simulator::helper::constructRPYfromQuaternion( + geometry_msgs::build().x(-0.183).y(-0.5).z(0.5).w(0.683)); + EXPECT_VECTOR3_NEAR(rpy, vec, 1.0e-3); + } } -TEST(HELPER, DETECTION_SENSOR_CONFIGURATION) +/** + * @note Test basic functionality. Test construction correctness of a pose. + */ +TEST(helper, constructPose) { - const auto configuration = - traffic_simulator::helper::constructDetectionSensorConfiguration("ego", "test", 3); - simulation_api_schema::DetectionSensorConfiguration expect_configuration; - expect_configuration.set_architecture_type("test"); - expect_configuration.set_entity("ego"); - expect_configuration.set_update_duration(3.0); - EXPECT_DETECTION_SENSOR_CONFIGURATION_EQ(configuration, expect_configuration); + const auto actual_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(43.0).y(47.0).z(53.0)) + .orientation( + geometry_msgs::build().x(-0.183).y(-0.5).z(0.5).w(0.683)); + + const auto result_pose = traffic_simulator::helper::constructPose( + 43.0, 47.0, 53.0, -M_PI / 3.0, -M_PI / 6.0, M_PI / 2.0); + + EXPECT_POSE_NEAR(result_pose, actual_pose, 1.0e-3); } -TEST(HELPER, LIDAR_SENSOR_CONFIGURATION) +/** + * @note Test basic functionality. Test construction correctness of + * a lidar sensor with both VLP16 and VLP32 configurations. + */ +TEST(helper, constructLidarConfiguration) { EXPECT_NO_THROW(traffic_simulator::helper::constructLidarConfiguration( traffic_simulator::helper::LidarType::VLP16, "ego", "test")); @@ -93,6 +120,23 @@ TEST(HELPER, LIDAR_SENSOR_CONFIGURATION) traffic_simulator::helper::LidarType::VLP32, "ego", "test")); } +/** + * @note Test basic functionality. Test construction correctness of + * a detection sensor with a given configuration. + */ +TEST(helper, constructDetectionSensorConfiguration) +{ + simulation_api_schema::DetectionSensorConfiguration actual_configuration; + actual_configuration.set_architecture_type("test"); + actual_configuration.set_entity("ego"); + actual_configuration.set_update_duration(3.0); + + const auto result_configuration = + traffic_simulator::helper::constructDetectionSensorConfiguration("ego", "test", 3.0); + + EXPECT_DETECTION_SENSOR_CONFIGURATION_EQ(result_configuration, actual_configuration); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index ba5f24b86c6..a12b5e33328 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -26,6 +26,9 @@ #include "catalogs.hpp" #include "expect_eq_macros.hpp" +constexpr auto convertDegToRad(const double deg) -> double { return deg / 180.0 * M_PI; } +constexpr auto convertRadToDeg(const double rad) -> double { return rad * 180.0 / M_PI; } + auto makePoint(const double x, const double y, const double z = 0.0) -> geometry_msgs::msg::Point { return geometry_msgs::build().x(x).y(y).z(z); @@ -38,6 +41,15 @@ auto makeBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg .dimensions(geometry_msgs::build().x(4.0).y(2.0).z(1.5)); } +auto makeCustom2DBoundingBox( + const double x, const double y, const double x_offset = 0.0, const double y_offset = 0.0) + -> traffic_simulator_msgs::msg::BoundingBox +{ + return traffic_simulator_msgs::build() + .center(geometry_msgs::build().x(x_offset).y(y_offset).z(0.0)) + .dimensions(geometry_msgs::build().x(x).y(y).z(0.0)); +} + auto makeSmallBoundingBox(const double center_y = 0.0) -> traffic_simulator_msgs::msg::BoundingBox { return traffic_simulator_msgs::build() @@ -51,6 +63,18 @@ auto makeQuaternionFromYaw(const double yaw) -> geometry_msgs::msg::Quaternion geometry_msgs::build().x(0.0).y(0.0).z(yaw)); } +auto makePose(const double x, const double y, const double yaw_deg) -> geometry_msgs::msg::Pose +{ + /** + * @note +x axis is 0 degrees; +y axis is 90 degrees + */ + return geometry_msgs::build() + .position(geometry_msgs::build().x(x).y(y).z(0.0)) + .orientation(math::geometry::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z( + convertDegToRad(yaw_deg)))); +} + auto makePose( geometry_msgs::msg::Point position, geometry_msgs::msg::Quaternion orientation = geometry_msgs::msg::Quaternion()) @@ -63,7 +87,8 @@ auto makePose( auto makeHdMapUtilsSharedPointer() -> std::shared_ptr { return std::make_shared( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", geographic_msgs::build() .latitude(35.9037067912303) .longitude(139.9337945139059) diff --git a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp index 96e57887d90..9a88e0ab995 100644 --- a/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp +++ b/simulation/traffic_simulator/test/src/simulation_clock/test_simulation_clock.cpp @@ -14,6 +14,7 @@ #include +#include #include /** @@ -21,7 +22,7 @@ * Test initialization logic by calling update without initialized clock * - the goal is to verify that mandatory initialization works. */ -TEST(SimulationClock, Initialize) +TEST(SimulationClock, SimulationClock) { auto simulation_clock = traffic_simulator::SimulationClock(true, 1.0, 10.0); @@ -31,7 +32,7 @@ TEST(SimulationClock, Initialize) EXPECT_TRUE(simulation_clock.started()); simulation_clock.update(); - EXPECT_THROW(simulation_clock.start(), std::runtime_error); + EXPECT_THROW(simulation_clock.start(), common::SimulationError); } /** diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index b77ec381cab..55ac100b94f 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -20,16 +20,23 @@ #include #include +using TrafficLight = traffic_simulator::TrafficLight; +using Color = TrafficLight::Color; +using Status = TrafficLight::Status; +using Shape = TrafficLight::Shape; +using Bulb = TrafficLight::Bulb; + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } -TEST(TrafficLight, Color) +/** + * @note Test object creation correctness. + */ +TEST(Color, Color) { - using Color = traffic_simulator::TrafficLight::Color; - { const auto color = Color("green"); @@ -76,10 +83,71 @@ TEST(TrafficLight, Color) } } -TEST(TrafficLight, Status) +/** + * @note Test basic functionality. Test whether the function + * creates Color object appropriate to the argument. + */ +TEST(Color, make) { - using Status = traffic_simulator::TrafficLight::Status; + { + const auto color = Color::make("green"); + + EXPECT_TRUE(color == Color::green); + EXPECT_TRUE(color.is(Color::green)); + EXPECT_TRUE(boost::lexical_cast(color) == Color::green); + EXPECT_TRUE(boost::lexical_cast(color) == "green"); + } + { + const auto color = Color::make("yellow"); + + EXPECT_TRUE(color == Color::yellow); + EXPECT_TRUE(color.is(Color::yellow)); + EXPECT_TRUE(boost::lexical_cast(color) == Color::yellow); + EXPECT_TRUE(boost::lexical_cast(color) == "yellow"); + } + + { + const auto color = Color::make("red"); + + EXPECT_TRUE(color == Color::red); + EXPECT_TRUE(color.is(Color::red)); + EXPECT_TRUE(boost::lexical_cast(color) == Color::red); + EXPECT_TRUE(boost::lexical_cast(color) == "red"); + } + + { + const auto color = Color::make("white"); + + EXPECT_TRUE(color == Color::white); + EXPECT_TRUE(color.is(Color::white)); + EXPECT_TRUE(boost::lexical_cast(color) == Color::white); + EXPECT_TRUE(boost::lexical_cast(color) == "white"); + } + + { + const auto color = Color::make("amber"); + + EXPECT_TRUE(color == Color::yellow); + EXPECT_TRUE(color.is(Color::yellow)); + EXPECT_TRUE(boost::lexical_cast(color) == Color::yellow); + EXPECT_TRUE(boost::lexical_cast(color) == "yellow"); + } +} + +/** + * @note Test basic functionality. Test function behavior when called with invalid name. + */ +TEST(Color, make_wrong) +{ + EXPECT_THROW(traffic_simulator::TrafficLight::Color::make("wrong_color"), common::SyntaxError); +} + +/** + * @note Test object creation correctness. + */ +TEST(Status, Status) +{ { const auto status = Status("solidOn"); @@ -117,10 +185,61 @@ TEST(TrafficLight, Status) } } -TEST(TrafficLight, Shape) +/** + * @note Test basic functionality. Test whether the function creates Status object appropriate to the argument. + */ +TEST(Status, make) +{ + { + const auto status = Status::make("solidOn"); + + EXPECT_TRUE(status == Status::solid_on); + EXPECT_TRUE(status.is(Status::solid_on)); + EXPECT_TRUE(boost::lexical_cast(status) == Status::solid_on); + EXPECT_TRUE(boost::lexical_cast(status) == "solidOn"); + } + + { + const auto status = Status::make("solidOff"); + + EXPECT_TRUE(status == Status::solid_off); + EXPECT_TRUE(status.is(Status::solid_off)); + EXPECT_TRUE(boost::lexical_cast(status) == Status::solid_off); + EXPECT_TRUE(boost::lexical_cast(status) == "solidOff"); + } + + { + const auto status = Status::make("flashing"); + + EXPECT_TRUE(status == Status::flashing); + EXPECT_TRUE(status.is(Status::flashing)); + EXPECT_TRUE(boost::lexical_cast(status) == Status::flashing); + EXPECT_TRUE(boost::lexical_cast(status) == "flashing"); + } + + { + const auto status = Status::make("unknown"); + + EXPECT_TRUE(status == Status::unknown); + EXPECT_TRUE(status.is(Status::unknown)); + EXPECT_TRUE(boost::lexical_cast(status) == Status::unknown); + EXPECT_TRUE(boost::lexical_cast(status) == "unknown"); + } +} + +/** + * @note Test basic functionality. Test function behavior when called with invalid name. + */ +TEST(Status, make_wrong) { - using Shape = traffic_simulator::TrafficLight::Shape; + EXPECT_THROW(traffic_simulator::TrafficLight::Status::make("wrong_status"), common::SyntaxError); +} +/** + * @note Test object creation correctness. + */ +TEST(Shape, Shape) +{ { const auto shape = Shape("circle"); @@ -232,207 +351,11 @@ TEST(TrafficLight, Shape) } } -TEST(TrafficLight, Bulb) -{ - using TrafficLight = traffic_simulator::TrafficLight; - using Color = TrafficLight::Color; - using Status = TrafficLight::Status; - using Shape = TrafficLight::Shape; - using Bulb = TrafficLight::Bulb; - - // clang-format off - static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::yellow, Status::solid_on, Shape::circle ).hash() == 0b0000'0001'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::red, Status::solid_on, Shape::circle ).hash() == 0b0000'0010'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::white, Status::solid_on, Shape::circle ).hash() == 0b0000'0011'0000'0000'0000'0000'0000'0000); - - static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::solid_off, Shape::circle ).hash() == 0b0000'0000'0000'0001'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::flashing, Shape::circle ).hash() == 0b0000'0000'0000'0010'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::unknown, Shape::circle ).hash() == 0b0000'0000'0000'0011'0000'0000'0000'0000); - - static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); - static_assert(Bulb(Color::green, Status::solid_on, Shape::cross ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0001); - static_assert(Bulb(Color::green, Status::solid_on, Shape::left ).hash() == 0b0000'0000'0000'0000'0000'1000'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::down ).hash() == 0b0000'0000'0000'0000'0000'0100'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::up ).hash() == 0b0000'0000'0000'0000'0000'0010'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::right ).hash() == 0b0000'0000'0000'0000'0000'0001'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_left ).hash() == 0b0000'0000'0000'0000'0000'1100'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_left ).hash() == 0b0000'0000'0000'0000'0000'1010'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_right).hash() == 0b0000'0000'0000'0000'0000'0101'0000'0010); - static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_right).hash() == 0b0000'0000'0000'0000'0000'0011'0000'0010); - // clang-format on - - { - constexpr auto bulb = Bulb(Color::red, Status::flashing, Shape::circle); - - EXPECT_TRUE(bulb.is(Color::red)); - EXPECT_TRUE(bulb.is(Status::flashing)); - EXPECT_TRUE(bulb.is(Shape::circle)); - EXPECT_TRUE(bulb.is(Shape::Category::circle)); - } - - { - constexpr auto bulb = Bulb(Color::green, Status::solid_on, Shape::right); - - EXPECT_TRUE(bulb.is(Color::green)); - EXPECT_TRUE(bulb.is(Status::solid_on)); - EXPECT_TRUE(bulb.is(Shape::right)); - EXPECT_TRUE(bulb.is(Shape::Category::arrow)); - } - - { - const auto bulb = Bulb("red flashing circle"); - - EXPECT_TRUE(bulb.is(Color::red)); - EXPECT_TRUE(bulb.is(Status::flashing)); - EXPECT_TRUE(bulb.is(Shape::circle)); - EXPECT_TRUE(bulb.is(Shape::Category::circle)); - } - - { - const auto bulb = Bulb("red flashing"); - - EXPECT_TRUE(bulb.is(Color::red)); - EXPECT_TRUE(bulb.is(Status::flashing)); - EXPECT_TRUE(bulb.is(Shape::circle)); - EXPECT_TRUE(bulb.is(Shape::Category::circle)); - } - - { - const auto bulb = Bulb("red"); - - EXPECT_TRUE(bulb.is(Color::red)); - EXPECT_TRUE(bulb.is(Status::solid_on)); - EXPECT_TRUE(bulb.is(Shape::circle)); - EXPECT_TRUE(bulb.is(Shape::Category::circle)); - } - - { - const auto bulb = Bulb("green solidOn right"); - - EXPECT_TRUE(bulb.is(Color::green)); - EXPECT_TRUE(bulb.is(Status::solid_on)); - EXPECT_TRUE(bulb.is(Shape::right)); - EXPECT_TRUE(bulb.is(Shape::Category::arrow)); - } - - { - const auto bulb = Bulb("green right"); - - EXPECT_TRUE(bulb.is(Color::green)); - EXPECT_TRUE(bulb.is(Status::solid_on)); - EXPECT_TRUE(bulb.is(Shape::right)); - EXPECT_TRUE(bulb.is(Shape::Category::arrow)); - } -} - -TEST(TrafficLight, TrafficLight) -{ - using TrafficLight = traffic_simulator::TrafficLight; - using Color = TrafficLight::Color; - using Status = TrafficLight::Status; - using Shape = TrafficLight::Shape; - - hdmap_utils::HdMapUtils map_manager( - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", - []() { - geographic_msgs::msg::GeoPoint geo_point; - geo_point.latitude = 35.61836750154; - geo_point.longitude = 139.78066608243; - return geo_point; - }()); - - { - auto traffic_light = TrafficLight(34802, map_manager); - - traffic_light.emplace(Color::red, Status::flashing, Shape::circle); - traffic_light.emplace(Color::green, Status::solid_on, Shape::right); - - EXPECT_TRUE(traffic_light.contains(Color::red, Status::flashing, Shape::circle)); - EXPECT_TRUE(traffic_light.contains(Color::green, Status::solid_on, Shape::right)); - } - - { - auto traffic_light = TrafficLight(34802, map_manager); - - traffic_light.emplace("red flashing circle"); - traffic_light.emplace("green solidOn right"); - - EXPECT_TRUE(traffic_light.contains(Color::red, Status::flashing, Shape::circle)); - EXPECT_TRUE(traffic_light.contains(Color::green, Status::solid_on, Shape::right)); - } - - { - auto traffic_light = TrafficLight(34802, map_manager); - - traffic_light.set("red flashing circle, green solidOn right"); - - EXPECT_TRUE(traffic_light.contains(Color::red, Status::flashing, Shape::circle)); - EXPECT_TRUE(traffic_light.contains(Color::green, Status::solid_on, Shape::right)); - } -} - /** - * @note Test basic functionality. Test whether the function - * creates Color object appropriate to the argument. + * @note Test basic functionality. Test whether the function creates Shape object appropriate to the argument. */ -TEST(TrafficLight, Color_make) +TEST(Shape, make) { - using Color = traffic_simulator::TrafficLight::Color; - { - const auto color = Color::make("green"); - - EXPECT_TRUE(color == Color::green); - EXPECT_TRUE(color.is(Color::green)); - EXPECT_TRUE(boost::lexical_cast("green") == Color::green); - EXPECT_TRUE(boost::lexical_cast(color) == "green"); - } - - { - const auto color = Color::make("yellow"); - - EXPECT_TRUE(color == Color::yellow); - EXPECT_TRUE(color.is(Color::yellow)); - EXPECT_TRUE(boost::lexical_cast("yellow") == Color::yellow); - EXPECT_TRUE(boost::lexical_cast(color) == "yellow"); - } - - { - const auto color = Color::make("red"); - - EXPECT_TRUE(color == Color::red); - EXPECT_TRUE(color.is(Color::red)); - EXPECT_TRUE(boost::lexical_cast("red") == Color::red); - EXPECT_TRUE(boost::lexical_cast(color) == "red"); - } - - { - const auto color = Color::make("white"); - - EXPECT_TRUE(color == Color::white); - EXPECT_TRUE(color.is(Color::white)); - EXPECT_TRUE(boost::lexical_cast("white") == Color::white); - EXPECT_TRUE(boost::lexical_cast(color) == "white"); - } - - { - const auto color = Color::make("amber"); - - EXPECT_TRUE(color == Color::yellow); - EXPECT_TRUE(color.is(Color::yellow)); - EXPECT_TRUE(boost::lexical_cast("amber") == Color::yellow); - EXPECT_TRUE(boost::lexical_cast(color) == "yellow"); - } -} - -/** - * @note Test basic functionality. Test whether the function creates Color object appropriate to the argument. - */ -TEST(TrafficLight, Shape_make) -{ - using Shape = traffic_simulator::TrafficLight::Shape; - { const auto shape = Shape::make("circle"); @@ -545,59 +468,116 @@ TEST(TrafficLight, Shape_make) } /** - * @note Test basic functionality. Test whether the function creates Status object appropriate to the argument. + * @note Test basic functionality. Test function behavior when called with invalid name. + */ +TEST(Shape, make_wrong) +{ + EXPECT_THROW(traffic_simulator::TrafficLight::Shape::make("wrong_shape"), common::SyntaxError); +} + +/** + * @note Test hashing function. An object must be assigned the same hash every time. */ -TEST(TrafficLight, Status_make) +TEST(Bulb, hash) { - using Status = traffic_simulator::TrafficLight::Status; + // clang-format off + static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::yellow, Status::solid_on, Shape::circle ).hash() == 0b0000'0001'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::red, Status::solid_on, Shape::circle ).hash() == 0b0000'0010'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::white, Status::solid_on, Shape::circle ).hash() == 0b0000'0011'0000'0000'0000'0000'0000'0000); + + static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::solid_off, Shape::circle ).hash() == 0b0000'0000'0000'0001'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::flashing, Shape::circle ).hash() == 0b0000'0000'0000'0010'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::unknown, Shape::circle ).hash() == 0b0000'0000'0000'0011'0000'0000'0000'0000); + + static_assert(Bulb(Color::green, Status::solid_on, Shape::circle ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0000); + static_assert(Bulb(Color::green, Status::solid_on, Shape::cross ).hash() == 0b0000'0000'0000'0000'0000'0000'0000'0001); + static_assert(Bulb(Color::green, Status::solid_on, Shape::left ).hash() == 0b0000'0000'0000'0000'0000'1000'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::down ).hash() == 0b0000'0000'0000'0000'0000'0100'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::up ).hash() == 0b0000'0000'0000'0000'0000'0010'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::right ).hash() == 0b0000'0000'0000'0000'0000'0001'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_left ).hash() == 0b0000'0000'0000'0000'0000'1100'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_left ).hash() == 0b0000'0000'0000'0000'0000'1010'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::lower_right).hash() == 0b0000'0000'0000'0000'0000'0101'0000'0010); + static_assert(Bulb(Color::green, Status::solid_on, Shape::upper_right).hash() == 0b0000'0000'0000'0000'0000'0011'0000'0010); + // clang-format on +} +/** + * @note Test basic functionality. Test Bulb comparisons with different configurations. + */ +TEST(Bulb, is) +{ { - const auto status = Status::make("solidOn"); + constexpr auto bulb = Bulb(Color::red, Status::flashing, Shape::circle); - EXPECT_TRUE(status == Status::solid_on); - EXPECT_TRUE(status.is(Status::solid_on)); - EXPECT_TRUE(boost::lexical_cast("solidOn") == Status::solid_on); - EXPECT_TRUE(boost::lexical_cast(status) == "solidOn"); + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::flashing)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); } { - const auto status = Status::make("solidOff"); + constexpr auto bulb = Bulb(Color::green, Status::solid_on, Shape::right); - EXPECT_TRUE(status == Status::solid_off); - EXPECT_TRUE(status.is(Status::solid_off)); - EXPECT_TRUE(boost::lexical_cast("solidOff") == Status::solid_off); - EXPECT_TRUE(boost::lexical_cast(status) == "solidOff"); + EXPECT_TRUE(bulb.is(Color::green)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::right)); + EXPECT_TRUE(bulb.is(Shape::Category::arrow)); } { - const auto status = Status::make("flashing"); + const auto bulb = Bulb("red flashing circle"); - EXPECT_TRUE(status == Status::flashing); - EXPECT_TRUE(status.is(Status::flashing)); - EXPECT_TRUE(boost::lexical_cast("flashing") == Status::flashing); - EXPECT_TRUE(boost::lexical_cast(status) == "flashing"); + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::flashing)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); } { - const auto status = Status::make("unknown"); + const auto bulb = Bulb("red flashing"); - EXPECT_TRUE(status == Status::unknown); - EXPECT_TRUE(status.is(Status::unknown)); - EXPECT_TRUE(boost::lexical_cast("unknown") == Status::unknown); - EXPECT_TRUE(boost::lexical_cast(status) == "unknown"); + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::flashing)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); + } + + { + const auto bulb = Bulb("red"); + + EXPECT_TRUE(bulb.is(Color::red)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::circle)); + EXPECT_TRUE(bulb.is(Shape::Category::circle)); + } + + { + const auto bulb = Bulb("green solidOn right"); + + EXPECT_TRUE(bulb.is(Color::green)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::right)); + EXPECT_TRUE(bulb.is(Shape::Category::arrow)); + } + + { + const auto bulb = Bulb("green right"); + + EXPECT_TRUE(bulb.is(Color::green)); + EXPECT_TRUE(bulb.is(Status::solid_on)); + EXPECT_TRUE(bulb.is(Shape::right)); + EXPECT_TRUE(bulb.is(Shape::Category::arrow)); } } /** - * @note Test basic functionality. Test whether the function creates Color object appropriate to the argument. + * @note Test basic functionality. Test whether the function creates Bulb object appropriate to the argument. */ -TEST(TrafficLight, Bulb_make) +TEST(Bulb, make) { - using TrafficLight = traffic_simulator::TrafficLight; - using Color = TrafficLight::Color; - using Status = TrafficLight::Status; - using Shape = TrafficLight::Shape; - using Bulb = TrafficLight::Bulb; { constexpr auto bulb = Bulb(Color::red, Status::flashing, Shape::circle); @@ -662,17 +642,23 @@ TEST(TrafficLight, Bulb_make) } } +/** + * @note Test basic functionality. Test function behavior when called with invalid name. + */ +TEST(Bulb, make_wrong) +{ + EXPECT_THROW(Bulb("red flashing wrong_shape"), common::SyntaxError); + EXPECT_THROW(Bulb("red wrong_status circle"), common::SyntaxError); + EXPECT_THROW(Bulb("wrong_color flashing circle"), common::SyntaxError); + EXPECT_THROW(Bulb("wrong_color wrong_status wrong_shape"), common::SyntaxError); +} + /** * @note Test basic functionality. Test whether the TrafficLight message * is constructed configured according to the Bulb object. */ -TEST(TrafficLight, Bulb_trafficLightMessageConversion) +TEST(Bulb, operator_TrafficLight) { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; - using Bulb = traffic_simulator::TrafficLight::Bulb; - { constexpr auto bulb = Bulb(Color::red, Status::flashing, Shape::circle); std::ostringstream oss; @@ -723,39 +709,59 @@ TEST(TrafficLight, Bulb_trafficLightMessageConversion) } } -/** - * @note Test basic functionality. Test function behavior when called with invalid name. - */ -TEST(TrafficLight, Color_make_wrong) +class TrafficLightTest : public testing::Test { - EXPECT_THROW(traffic_simulator::TrafficLight::Color::make("wrong_color"), common::SyntaxError); -} +protected: + TrafficLightTest() + : map_manager( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)) + { + } + hdmap_utils::HdMapUtils map_manager; +}; /** - * @note Test basic functionality. Test function behavior when called with invalid name. + * @note test if function correctly determines if a given bulb + * is in the bulbs vector given a Color, Status, Shape triple. */ -TEST(TrafficLight, Shape_make_wrong) +TEST_F(TrafficLightTest, contains_colorStatusShape) { - EXPECT_THROW(traffic_simulator::TrafficLight::Shape::make("wrong_shape"), common::SyntaxError); -} + { + auto traffic_light = TrafficLight(34802, map_manager); -/** - * @note Test basic functionality. Test function behavior when called with invalid name. - */ -TEST(TrafficLight, Status_make_wrong) -{ - EXPECT_THROW(traffic_simulator::TrafficLight::Status::make("wrong_status"), common::SyntaxError); + traffic_light.bulbs.emplace(Color::red, Status::flashing, Shape::circle); + traffic_light.bulbs.emplace(Color::green, Status::solid_on, Shape::right); + + ASSERT_TRUE(traffic_light.bulbs.size() == static_cast(2)); + EXPECT_TRUE(traffic_light.contains(Color::red, Status::flashing, Shape::circle)); + EXPECT_TRUE(traffic_light.contains(Color::green, Status::solid_on, Shape::right)); + } + + { + auto traffic_light = TrafficLight(34802, map_manager); + + traffic_light.bulbs.emplace("red flashing circle"); + traffic_light.bulbs.emplace("green solidOn right"); + + EXPECT_TRUE(traffic_light.contains(Color::red, Status::flashing, Shape::circle)); + EXPECT_TRUE(traffic_light.contains(Color::green, Status::solid_on, Shape::right)); + } } /** - * @note Test basic functionality. Test function behavior when called with invalid name. + * @note Test function behavior with a valid string. */ -TEST(TrafficLight, Bulb_make_wrong) +TEST_F(TrafficLightTest, set_valid) { - using Bulb = traffic_simulator::TrafficLight::Bulb; + auto traffic_light = TrafficLight(34802, map_manager); - EXPECT_THROW(Bulb("red flashing wrong_shape"), common::SyntaxError); - EXPECT_THROW(Bulb("red wrong_status circle"), common::SyntaxError); - EXPECT_THROW(Bulb("wrong_color flashing circle"), common::SyntaxError); - EXPECT_THROW(Bulb("wrong_color wrong_status wrong_shape"), common::SyntaxError); + traffic_light.set("red flashing circle, green solidOn right"); + + EXPECT_TRUE(traffic_light.contains(Color::red, Status::flashing, Shape::circle)); + EXPECT_TRUE(traffic_light.contains(Color::green, Status::solid_on, Shape::right)); } diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp index 86aea5345b0..f7cea5e115e 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp @@ -34,8 +34,8 @@ class TrafficLightsTest : public testing::Test const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsTest"); - const std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + const std::string path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; const std::shared_ptr hdmap_utils_ptr = std::make_shared( diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp index 8872be966b1..3e85d191319 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights_internal.cpp @@ -35,8 +35,8 @@ class TrafficLightsInternalTest : public testing::Test const rclcpp::Node::SharedPtr node_ptr = rclcpp::Node::make_shared("TrafficLightsInternalTest"); - const std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; + const std::string path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; const std::shared_ptr hdmap_utils_ptr = std::make_shared( diff --git a/simulation/traffic_simulator/test/src/utils/CMakeLists.txt b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt new file mode 100644 index 00000000000..80d35ff1bd2 --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt @@ -0,0 +1,6 @@ + +ament_add_gtest(test_distance test_distance.cpp) +target_link_libraries(test_distance traffic_simulator) + +ament_add_gtest(test_pose test_pose.cpp) +target_link_libraries(test_pose traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp new file mode 100644 index 00000000000..7d59aba49b5 --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -0,0 +1,708 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../helper_functions.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +class distanceTest_FourTrackHighway : public testing::Test +{ +protected: + distanceTest_FourTrackHighway() + : hdmap_utils_ptr(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/four_track_highway/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.22312494055522) + .longitude(138.8024583466017) + .altitude(0.0))) + { + } + std::shared_ptr hdmap_utils_ptr; +}; + +class distanceTest_StandardMap : public testing::Test +{ +protected: + distanceTest_StandardMap() + : hdmap_utils_ptr(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0))) + { + } + std::shared_ptr hdmap_utils_ptr; +}; + +class distanceTest_Intersection : public testing::Test +{ +protected: + distanceTest_Intersection() + : hdmap_utils_ptr(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/intersection/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.64200728302) + .longitude(139.74821144562) + .altitude(0.0))) + { + } + std::shared_ptr hdmap_utils_ptr; +}; + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is impossible to calculate the distance, e.g. not connected lanelets + * and with allow_lane_change = false. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_noChange) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance + * and with allow_lane_change = false. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_noChange) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(201L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is impossible to calculate the distance, e.g. not connected lanelets + * and with allow_lane_change = true. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_change) +{ + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance + * and with allow_lane_change = true. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_change) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + constexpr double approx_distance = -3.0; + constexpr double tolerance = 0.5; + { + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), approx_distance, tolerance); + } + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), approx_distance, tolerance); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance, but matching_distance is too small. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_impossible_matching) +{ + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, 2.0, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test if the function correctly uses getLateralDistance. Test with a scenario + * in which it is possible to calculate the distance and matching_distance is large. + */ +TEST_F(distanceTest_FourTrackHighway, lateralDistance_possible_matching) +{ + const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( + 3002184L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + + { + const auto result = + traffic_simulator::distance::lateralDistance(pose_from, pose_to, 3.0, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), -3.0, 0.5); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = false + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_noChange_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81595.44, 50006.09, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81584.48, 50084.76, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = false + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noChange) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3800.05, 73715.77, 30.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(3841.26, 73748.80, 110.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 60.0, 1.0); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = false + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81585.79, 50042.62, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81588.34, 50083.23, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, false, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = false + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_adjacent_noOpposite_noChange) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.02, 50065.76, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.61, 50045.16, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, false, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 20.0, 1.0); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = true + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81595.47, 49982.80, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81612.35, 50015.63, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = false, + * include_opposite_direction = false, allow_lane_change = true + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_FourTrackHighway, longitudinalDistance_noAdjacent_noOpposite_change) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81592.96, 49997.94, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 145.0, 1.0); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81587.31, 50165.57, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 178.0, 1.0); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = true + * in an impossible scenario, e.g. no path. + */ +TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_change_false) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86736.13, 44969.63, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86732.06, 44976.58, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + EXPECT_FALSE(result.has_value()); + } +} + +/** + * @note Test calculation correctness with include_adjacent_lanelet = true, + * include_opposite_direction = false, allow_lane_change = true + * in a scenario that meets those criteria. + */ +TEST_F(distanceTest_Intersection, longitudinalDistance_adjacent_noOpposite_change) +{ + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86637.19, 44967.35, 340.0), false, hdmap_utils_ptr); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 103.0, 1.0); + } + { + const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86719.94, 44957.20, 210.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( + makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); + ASSERT_TRUE(pose_from.has_value()); + + const auto result = traffic_simulator::distance::longitudinalDistance( + pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result.value(), 131.0, 1.0); + } +} + +/** + * @note Test equality with math::geometry::getPolygonDistance + * function result on intersecting bounding boxes. + */ +TEST(distance, boundingBoxDistance_intersection) +{ + const auto pose_from = makePose(100.0, 100.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto bounding_box_from = makeCustom2DBoundingBox(30.0, 1.0); + const auto bounding_box_to = makeCustom2DBoundingBox(1.0, 30.0); + + const auto result_distance = traffic_simulator::distance::boundingBoxDistance( + pose_from, bounding_box_from, pose_to, bounding_box_to); + const auto actual_distance = + math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); + EXPECT_FALSE(result_distance.has_value()); + EXPECT_FALSE(actual_distance.has_value()); +} + +/** + * @note Test equality with math::geometry::getPolygonDistance + * function result on disjoint bounding boxes. + */ +TEST(distance, boundingBoxDistance_disjoint) +{ + const auto pose_from = makePose(100.0, 100.0, 0.0); + const auto pose_to = makePose(120.0, 100.0, 90.0); + const auto bounding_box_from = makeCustom2DBoundingBox(1.0, 30.0); + const auto bounding_box_to = makeCustom2DBoundingBox(30.0, 1.0); + + const auto result_distance = traffic_simulator::distance::boundingBoxDistance( + pose_from, bounding_box_from, pose_to, bounding_box_to); + const auto actual_distance = + math::geometry::getPolygonDistance(pose_from, bounding_box_from, pose_to, bounding_box_to); + ASSERT_TRUE(result_distance.has_value()); + ASSERT_TRUE(actual_distance.has_value()); + EXPECT_NEAR( + actual_distance.value(), result_distance.value(), std::numeric_limits::epsilon()); +} + +/** + * @note Test calculation correctness with lanelet::Id. + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_single) +{ + constexpr lanelet::Id lanelet_id = 34741L; + constexpr double tolerance = 0.1; + { + const auto pose = makePose(3818.33, 73726.18, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 0.5, tolerance); + } + { + const auto pose = makePose(3816.89, 73723.09, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.6, tolerance); + } + { + const auto pose = makePose(3813.42, 73721.11, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.7, tolerance); + } + { + const auto pose = makePose(3813.42, 73721.11, 120.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 1.3, tolerance); + } + { + const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 1.4, tolerance); + } + { + const auto pose = makePose(3810.99, 73721.40, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.4, tolerance); + } + { + const auto pose = makePose(3680.81, 73757.27, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, 34684L, hdmap_utils_ptr); + EXPECT_NEAR(result, 5.1, tolerance); + } + { + const auto pose = makePose(3692.79, 73753.00, 30.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, 34684L, hdmap_utils_ptr); + EXPECT_NEAR(result, 7.2, tolerance); + } +} + +/** + * @note Test calculation correctness with a vector containing multiple lanelets. + * Test equality with the minimum of distanceToLeftLaneBound results (lanelet::Id overload). + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_multipleVector) +{ + const auto lanelet_ids = lanelet::Ids{34603L, 34600L, 34621L, 34741L}; + const auto pose = makePose(3836.16, 73757.99, 120.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = std::transform_reduce( + lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), + [](const double lhs, const double rhs) { return std::min(lhs, rhs); }, + [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { + return traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + }); + const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 1.4, 0.1); +} + +/** + * @note Test calculation correctness with a vector containing a single lanelet. + * Test equality with the distanceToLeftLaneBound results (lanelet::Id overload). + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_singleVector) +{ + constexpr lanelet::Id lanelet_id = 34426L; + const auto pose = makePose(3693.34, 73738.37, 300.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result_distance = traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 1.8, 0.1); +} + +/** + * @note Test function behavior with an empty vector. + */ +TEST_F(distanceTest_StandardMap, distanceToLeftLaneBound_emptyVector) +{ + const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + EXPECT_THROW( + traffic_simulator::distance::distanceToLeftLaneBound( + pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + common::SemanticError); +} + +/** + * @note Test calculation correctness with lanelet::Id. + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_single) +{ + constexpr lanelet::Id lanelet_id = 660L; + constexpr double tolerance = 0.1; + { + const auto pose = makePose(86651.84, 44941.47, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 4.1, tolerance); + } + { + const auto pose = makePose(86653.05, 44946.74, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 0.6, tolerance); + } + { + const auto pose = makePose(86651.47, 44941.07, 120.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 4.3, tolerance); + } + { + const auto pose = makePose(86651.47, 44941.07, 210.0); + const auto bounding_box = makeCustom2DBoundingBox(3.0, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 3.1, tolerance); + } + { + const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 1.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.0, tolerance); + } + { + const auto pose = makePose(86644.10, 44951.86, 150.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, -1.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 1.1, tolerance); + } + { + const auto pose = makePose(86644.11, 44941.21, 0.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 11.2, tolerance); + } + { + const auto pose = makePose(86656.83, 44946.96, 0.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double result = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + EXPECT_NEAR(result, 2.6, tolerance); + } +} + +/** + * @note Test calculation correctness with a vector containing multiple lanelets. + * Test equality with the minimum of distanceToRightLaneBound results (lanelet::Id overload). + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_multipleVector) +{ + const auto lanelet_ids = lanelet::Ids{660L, 663L, 684L, 654L, 686L}; + const auto pose = makePose(86642.05, 44902.61, 60.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = std::transform_reduce( + lanelet_ids.cbegin(), lanelet_ids.cend(), std::numeric_limits::max(), + [](const double lhs, const double rhs) { return std::min(lhs, rhs); }, + [&pose, &bounding_box, this](const lanelet::Id lanelet_id) { + return traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + }); + const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_ids, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 2.7, 0.1); +} + +/** + * @note Test calculation correctness with a vector containing a single lanelet. + * Test equality with the distanceToRightLaneBound result (lanelet::Id overload). + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_singleVector) +{ + constexpr lanelet::Id lanelet_id = 654L; + const auto pose = makePose(86702.79, 44929.05, 150.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + const double actual_distance = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet_id, hdmap_utils_ptr); + const double result_distance = traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, {lanelet_id}, hdmap_utils_ptr); + EXPECT_NEAR(actual_distance, result_distance, std::numeric_limits::epsilon()); + EXPECT_NEAR(result_distance, 2.4, 0.1); +} + +/** + * @note Test function behavior with an empty vector. + */ +TEST_F(distanceTest_Intersection, distanceToRightLaneBound_emptyVector) +{ + const auto pose = makePose(3825.87, 73773.08, 135.0); + const auto bounding_box = makeCustom2DBoundingBox(0.1, 0.1, 0.0, 0.0); + EXPECT_THROW( + traffic_simulator::distance::distanceToRightLaneBound( + pose, bounding_box, lanelet::Ids{}, hdmap_utils_ptr), + common::SemanticError); +} diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp new file mode 100644 index 00000000000..a7132a31c62 --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -0,0 +1,670 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "../helper_functions.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +class PoseTest : public testing::Test +{ +protected: + PoseTest() + : hdmap_utils(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/four_track_highway/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.22312494055522) + .longitude(138.8024583466017) + .altitude(0.0))) + { + } + + std::shared_ptr hdmap_utils; +}; + +/** + * @note Test created object values. + */ +TEST(pose, quietNaNPose) +{ + const auto pose = traffic_simulator::pose::quietNaNPose(); + + EXPECT_TRUE(std::isnan(pose.position.x)); + EXPECT_TRUE(std::isnan(pose.position.y)); + EXPECT_TRUE(std::isnan(pose.position.z)); + + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.w, 1.0); +} + +/** + * @note Test created object values. + */ +TEST(pose, quietNaNLaneletPose) +{ + const auto pose = traffic_simulator::pose::quietNaNLaneletPose(); + + EXPECT_EQ(pose.lanelet_id, std::numeric_limits::max()); + EXPECT_TRUE(std::isnan(pose.s)); + EXPECT_TRUE(std::isnan(pose.offset)); + EXPECT_TRUE(std::isnan(pose.rpy.x)); + EXPECT_TRUE(std::isnan(pose.rpy.y)); + EXPECT_TRUE(std::isnan(pose.rpy.z)); +} + +/** + * @note Test canonicalization with a default constructed LaneletPose. + */ +TEST_F(PoseTest, canonicalize_default) +{ + const auto pose = + traffic_simulator::pose::canonicalize(traffic_simulator_msgs::msg::LaneletPose(), hdmap_utils); + + EXPECT_FALSE(pose.has_value()); +} + +/** + * @note Test canonicalization with an invalid LaneletPose. + */ +TEST_F(PoseTest, canonicalize_invalid) +{ + EXPECT_THROW( + traffic_simulator::pose::canonicalize( + traffic_simulator::pose::quietNaNLaneletPose(), hdmap_utils), + std::runtime_error); +} + +/** + * @note Test canonicalization with a valid constructed LaneletPose. + */ +TEST_F(PoseTest, canonicalize_valid) +{ + const auto pose = traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0); + std::optional canonicalized_pose; + + EXPECT_NO_THROW(canonicalized_pose = traffic_simulator::pose::canonicalize(pose, hdmap_utils)); + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_LANELET_POSE_EQ( + static_cast(canonicalized_pose.value()), pose); +} + +/** + * @note Test conversion to geometry_msgs::msg::Pose from CanonicalizedLaneletPose. + */ +TEST_F(PoseTest, toMapPose_CanonicalizedLaneletPose) +{ + const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_pose( + traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0), hdmap_utils); + + const geometry_msgs::msg::Pose pose = makePose( + makePoint(81585.1622, 50176.9202, 34.2595), + geometry_msgs::build().x(0.0).y(0.0).z(-0.6397).w(0.7686)); + + EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(canonicalized_pose), pose, 0.01); +} + +/** + * @note Test conversion to geometry_msgs::msg::Pose from LaneletPose with HdMapUtils pointer. + */ +TEST_F(PoseTest, toMapPose_LaneletPose) +{ + const auto lanelet_pose = traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0); + + const geometry_msgs::msg::Pose pose = makePose( + makePoint(81585.1622, 50176.9202, 34.2595), + geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + + EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(lanelet_pose, hdmap_utils), pose, 0.01); +} + +/** + * @note Test function behavior with a pose that can be canonicalized. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note Test function behavior with a pose that can not be canonicalized. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) +{ + const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils), std::nullopt); +} + +/** + * @note Test function behavior with a pose that can be canonicalized. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), true, 1.0, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note Test function behavior with a pose that can not be canonicalized, matching distance too large. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeSmallBoundingBox(), true, 0.0, hdmap_utils), + std::nullopt); +} + +/** + * @note Test function behavior with an empty unique_route_lanelets vector and a pose that can not be canonicalized. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) +{ + const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils), + std::nullopt); +} + +/** + * @note Test function behavior with an empty unique_route_lanelets vector and a pose that can be canonicalized. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note Test function behavior with a non empty unique_route_lanelets vector and a pose that can not be canonicalized. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) +{ + const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), {195}, true, 1.0, hdmap_utils), + std::nullopt); +} + +/** + * @note Test function behavior with a non empty unique_route_lanelets vector and a pose that can be canonicalized. + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), lanelet::Ids{195}, true, 1.0, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note Test calculation correctness with a non trivial example. + */ +TEST(pose, transformRelativePoseToGlobal) +{ + const geometry_msgs::msg::Pose pose_relative = makePose(makePoint(4.9969, -28.1026, 0.214)); + const geometry_msgs::msg::Pose pose_global = makePose(makePoint(81601.0571, 50099.0981, 34.595)); + const geometry_msgs::msg::Pose pose_rel_global = + makePose(makePoint(81606.054, 50070.9955, 34.809)); + + EXPECT_POSE_EQ( + traffic_simulator::pose::transformRelativePoseToGlobal(pose_global, pose_relative), + pose_rel_global); +} + +/** + * @note Test calculation correctness when only one pose is zeroed, the goal is to obtain a pose equal to the second argument. + */ +TEST(pose, relativePose_poses_zero) +{ + const auto from = makePose(makePoint(0.0, 0.0, 0.0)); + const auto to = makePose(makePoint(10.0, 51.0, 2.0)); + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_EQ(relative.value(), to); +} + +/** + * @note Test calculation correctness when both poses are zeroed. + */ +TEST(pose, relativePose_poses_zeros) +{ + const auto from = makePose(makePoint(0.0, 0.0, 0.0)); + const auto to = makePose(makePoint(0.0, 0.0, 0.0)); + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_EQ(relative.value(), to); +} + +/** + * @note Test calculation correctness with a non trivial example. + */ +TEST(pose, relativePose_poses_complex) +{ + const auto pose_relative = makePose(makePoint(4.9969, -28.1026, 0.214)); + const auto from = makePose(makePoint(81601.0571, 50099.0981, 34.595)); + const auto to = makePose(makePoint(81606.054, 50070.9955, 34.809)); + + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note Test calculation correctness with the overload. + */ +TEST_F(PoseTest, relativePose_canonicalized_end_position) +{ + const auto pose_relative = makePose( + makePoint(9.9999, -0.0009, 0.0126), + geometry_msgs::build().x(0).y(0).z(0).w(1)); + const auto from = makePose( + makePoint(81585.1622, 50176.9202, 34.2595), + geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note Test calculation correctness with the overload. + */ +TEST_F(PoseTest, relativePose_canonicalized_start_position) +{ + const auto pose_relative = makePose( + makePoint(145244.7916, 786706.3326, 0.0127), + geometry_msgs::build().x(0).y(0).z(0).w(1)); + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = makePose( + makePoint(881586.9767, 50167.0862, 34.2722), + geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note Test calculation correctness when bounding boxes are disjoint. + */ +TEST_F(PoseTest, boundingBoxRelativePose_disjoint) +{ + const auto pose_relative = makePose(makePoint(0.1108, -20.5955, 0.0)); + const auto from = makePose(makePoint(81600.3967, 50094.4298, 34.6759)); + const auto to = makePose(makePoint(81604.5076, 50071.8343, 40.8482)); + const auto bounding_box = makeBoundingBox(); + + const auto relative = + traffic_simulator::pose::boundingBoxRelativePose(from, bounding_box, to, bounding_box); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note Test calculation correctness when bounding boxes share an edge. + */ +TEST_F(PoseTest, boundingBoxRelativePose_commonEdge) +{ + const auto from = makePose(makePoint(81600, 50094, 34)); + const auto to = makePose(makePoint(81601, 50094, 34)); + const auto bounding_box = makeSmallBoundingBox(); + + const auto relative = + traffic_simulator::pose::boundingBoxRelativePose(from, bounding_box, to, bounding_box); + + EXPECT_FALSE(relative.has_value()); +} + +/** + * @note Test calculation correctness when bounding boxes intersect. + */ +TEST_F(PoseTest, boundingBoxRelativePose_intersect) +{ + const auto from = makePose(makePoint(81600, 50094, 34)); + const auto to = makePose(makePoint(81600.5, 50094, 34)); + const auto bounding_box = makeSmallBoundingBox(); + + const auto relative = + traffic_simulator::pose::boundingBoxRelativePose(from, bounding_box, to, bounding_box); + + EXPECT_FALSE(relative.has_value()); +} + +/** + * @note Test s-value calculation correctness when longitudinal distance between the poses can not be calculated. + */ +TEST_F(PoseTest, relativeLaneletPose_s_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + + EXPECT_TRUE(std::isnan(relative.s)); +} + +/** + * @note Test s-value calculation correctness when longitudinal distance between the poses can be calculated. + */ +TEST_F(PoseTest, relativeLaneletPose_s_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + + EXPECT_NEAR(relative.s, 107.74, 0.001); +} + +/** + * @note Test offset-value calculation correctness when lateral distance between the poses can not be calculated. + */ +TEST_F(PoseTest, relativeLaneletPose_offset_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + + EXPECT_TRUE(std::isnan(relative.offset)); +} + +/** + * @note Test offset-value calculation correctness when lateral distance between the poses can be calculated. + */ +TEST_F(PoseTest, relativeLaneletPose_offset_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, true, hdmap_utils); + + EXPECT_EQ(relative.offset, 1.0); +} + +/** + * @note Test s-value calculation correctness when longitudinal distance between the poses can not be calculated. + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto bounding_box = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, bounding_box, to, bounding_box, false, hdmap_utils); + + EXPECT_TRUE(std::isnan(relative.s)); +} + +/** + * @note Test s-value calculation correctness when longitudinal distance between the poses can be calculated. + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + const auto bounding_box = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, bounding_box, to, bounding_box, false, hdmap_utils); + + EXPECT_NEAR(relative.s, 103.74, 0.01); +} + +/** + * @note Test offset-value calculation correctness when lateral distance between the poses can not be calculated. + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto bounding_box = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, bounding_box, to, bounding_box, false, hdmap_utils); + + EXPECT_TRUE(std::isnan(relative.s)); +} + +/** + * @note Test offset-value calculation correctness when lateral distance between the poses can be calculated. + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + const auto bounding_box = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, bounding_box, to, bounding_box, false, hdmap_utils); + + EXPECT_EQ(relative.offset, 0.0); +} + +/** + * @note Test calculation correctness when the pose lies within the lanelet. + */ +TEST_F(PoseTest, isInLanelet_inside) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + EXPECT_TRUE(traffic_simulator::pose::isInLanelet( + pose, 195, std::numeric_limits::epsilon(), hdmap_utils)); +} + +/** + * @note Test calculation correctness when the pose lies outside the front of the lanelet, distance greater than the tolerance. + */ +TEST_F(PoseTest, isInLanelet_outsideFrontFar) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1.0, hdmap_utils)); +} + +/** + * @note Test calculation correctness when the pose lies outside the front of the lanelet, distance smaller than the tolerance. + */ +TEST_F(PoseTest, isInLanelet_outsideFrontClose) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0, hdmap_utils); + + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2.0, hdmap_utils)); +} + +/** + * @note Test calculation correctness when the pose lies outside the back of the lanelet, distance greater than the tolerance. + */ +TEST_F(PoseTest, isInLanelet_outsideBackFar) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 195, 2, hdmap_utils)); +} + +/** + * @note Test calculation correctness when the pose lies outside the back of the lanelet, distance smaller than the tolerance. + */ +TEST_F(PoseTest, isInLanelet_outsideBackClose) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0, hdmap_utils); + + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10.0, hdmap_utils)); +} + +/** + * @note Test calculation correctness when there are no following lanelets and the pose lies within the lanelet. + */ +TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002171, 31.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note Test calculation correctness when there is a single following lanelet and the pose lies within the lanelet. + */ +TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 5.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note Test calculation correctness when there is a single following lanelet and the pose lies after the end of the lanelet. + */ +TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 20.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note Test calculation correctness when there are multiple following lanelets and the pose lies within the lanelet. + */ +TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 5.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note Test calculation correctness when there are multiple following lanelets and the pose lies after the end of the lanelet. + */ +TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note Test function behavior when non existent lanelet::Id has been passed. + */ +TEST_F(PoseTest, laneletLength_invalid) +{ + EXPECT_THROW( + traffic_simulator::pose::laneletLength(10000000000000000, hdmap_utils), std::runtime_error); +} + +/** + * @note Test calculation correctness when a correct lanelet::Id has been passed. + */ +TEST_F(PoseTest, laneletLength_valid) +{ + EXPECT_NEAR(traffic_simulator::pose::laneletLength(195, hdmap_utils), 107.74, 0.01); +} diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 3c3effd653d..cddf7590824 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,271 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 6c1f3fee0ea..b920daf5a85 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 3.3.0 + 4.3.7 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/sonar-project.properties b/sonar-project.properties new file mode 100644 index 00000000000..529e1acf78a --- /dev/null +++ b/sonar-project.properties @@ -0,0 +1,13 @@ +sonar.projectKey=tier4_scenario_simulator_v2 +sonar.organization=tier4 + +# This is the name and version displayed in the SonarCloud UI. +sonar.projectName=scenario_simulator_v2 +sonar.projectVersion=1.0 + +sonar.sources=/src + +# Encoding of the source code. Default is default system encoding +sonar.sourceEncoding=UTF-8 + +sonar.cfamily.gcov.reportsPath=/lcov/total_coverage.info diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 26f3f6934c3..1984bb06507 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,293 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge pull request `#1386 `_ from tier4/RJD-1201/documentation_update + [RJD-1201] Update Quick start / Build instructions +* Merge branch 'master' into RJD-1201/documentation_update +* Fix: Update supported architecture types for random_test_runner + - Updated launch script + - Updated Usage.md +* Contributors: Masaya Kataoka, SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ + +4.1.1 (2024-09-03) +------------------ +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge pull request `#1358 `_ from tier4/RJD-1056-remove-npc-logic-started + Remove unused data members: npc_logic_started +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Trailing return type +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Forward isNpcLogicStarted to API and restore TestExecutor +* Remove npc_logic_started from API +* Contributors: DMoszynski, Dawid Moszynski, Masaya Kataoka, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge pull request `#1320 `_ from tier4/ref/RJD-1053-set-update-canonicalized-entity-status + ref(behavior_tree, traffic_simulator): move responsibility for canonicalization to traffic_simulator, simplify +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* feat(traffic_simulator): use CanonicalizedEntityStatus only with single constructor +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* tmp +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Contributors: Kotaro Yoshimoto + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* remove \n +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ +* Merge pull request `#1345 `_ from tier4/newline-character-literal + Avoid false detection of newline character literals by spell check. +* Lipsticks +* Add a comment explaining the background of the hack +* Split the string “\nnpc” into “\n” and “npc”. +* Contributors: Masaya Kataoka, yamacir-kit + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ + 3.3.0 (2024-07-23) ------------------ * Merge branch 'master' into feature/interpreter/entity_selection diff --git a/test_runner/random_test_runner/include/random_test_runner/data_types.hpp b/test_runner/random_test_runner/include/random_test_runner/data_types.hpp index b6dbf773b5a..e0c168c43dc 100644 --- a/test_runner/random_test_runner/include/random_test_runner/data_types.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/data_types.hpp @@ -166,7 +166,8 @@ struct fmt::formatter fmt::format_to( ctx.out(), "ego_start_position: {} ego_goal_position: {} " - "ego_goal_pose: {}\nnpc_descriptions:", + "ego_goal_pose: {}\n" // The spell checker detects the string as an illegal word, so it is necessary to split the string like this. + "npc_descriptions:", v.ego_start_position, v.ego_goal_position, v.ego_goal_pose); for (size_t idx = 0; idx < v.npcs_descriptions.size(); idx++) { fmt::format_to(ctx.out(), "[{}]: {}\n", idx, v.npcs_descriptions[idx]); diff --git a/test_runner/random_test_runner/launch/random_test.launch.py b/test_runner/random_test_runner/launch/random_test.launch.py index 62b6301bef6..21c0389e90a 100644 --- a/test_runner/random_test_runner/launch/random_test.launch.py +++ b/test_runner/random_test_runner/launch/random_test.launch.py @@ -40,7 +40,8 @@ def __init__(self): self.autoware_launch_arguments = { # autoware arguments # - "architecture_type": {"default": "awf/universe", "description": "Autoware architecture type", "values": "awf/universe"}, + "architecture_type": {"default": "awf/universe/20230906", "description": "Autoware architecture type", + "values": ["awf/universe/20230906", "awf/universe/20240605"]}, "sensor_model": {"default": "sample_sensor_kit", "description": "Ego sensor model"}, "vehicle_model": {"default": "sample_vehicle", "description": "Ego vehicle model"}, "autoware_launch_file": {"default": "planning_simulator.launch.xml", "description": "Launch file name for Autoware running"}, diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index e7b7b0f52a8..4c6923e4cb6 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 3.3.0 + 4.3.7 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 4ef9428c510..26c0ed6da84 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,305 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +4.3.7 (2024-09-27) +------------------ +* Merge branch 'master' into feature/sonar_cloud +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/sonar_cloud +* Contributors: Masaya Kataoka + +4.3.6 (2024-09-27) +------------------ + +4.3.5 (2024-09-27) +------------------ +* Merge branch 'master' into feature/lcov +* Contributors: Masaya Kataoka + +4.3.4 (2024-09-27) +------------------ +* Merge branch 'master' into RJD-1201/fix_quick_start +* Merge branch 'master' into RJD-1201/fix_quick_start +* Contributors: SzymonParapura + +4.3.3 (2024-09-26) +------------------ +* Merge pull request `#1369 `_ from tier4/fix/acquire-position-action + Fix/acquire position action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge branch 'master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Update the scenario to drive laps around the closed route +* Merge remote-tracking branch 'origin/master' into fix/acquire-position-action +* Add new test scenario `RoutingAction.AcquirePositionAction-continuous` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +4.3.2 (2024-09-24) +------------------ +* Merge branch 'master' into fix/imu_frame +* Merge branch 'master' into fix/imu_frame +* Contributors: Kotaro Yoshimoto + +4.3.1 (2024-09-19) +------------------ +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Merge branch 'master' into fix/fix-eigen-variable-definition +* Contributors: Masaya Kataoka + +4.3.0 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1201/documentation_update +* Contributors: SzymonParapura + +4.2.9 (2024-09-19) +------------------ +* Merge branch 'master' into RJD-1197/distance +* Merge branch 'master' into RJD-1197/distance +* resolve conflict +* Merge branch 'master' into RJD-1197/distance +* Contributors: Michał Ciasnocha, robomic + +4.2.8 (2024-09-18) +------------------ + +4.2.7 (2024-09-13) +------------------ + +4.2.6 (2024-09-13) +------------------ +* Merge branch 'master' into RJD-1197/pose_module +* Contributors: Masaya Kataoka + +4.2.5 (2024-09-12) +------------------ + +4.2.4 (2024-09-12) +------------------ + +4.2.3 (2024-09-11) +------------------ + +4.2.2 (2024-09-10) +------------------ +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Merge branch 'master' into RJD-1278/geometry-update +* Contributors: Masaya Kataoka, Michał Ciasnocha + +4.2.1 (2024-09-10) +------------------ + +4.2.0 (2024-09-09) +------------------ +* Merge pull request `#1362 `_ from tier4/feature/ros2-parameter-forwarding +* Move function `make_vehicle_parameters` into function `make_parameters` +* Move parameter `use_sim_time` into function `make_parameters` +* Simplify collection of `autoware.` prefixed parameters +* Remove data member `traffic_simulator::Configuration::rviz_config_path` +* Add feature to forward parameters prefixed with `autoware.` to Autoware +* Contributors: Kotaro Yoshimoto, yamacir-kit + +4.1.1 (2024-09-03) +------------------ +* Merge pull request `#1207 `_ from tier4/fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(use_sim_time): set default as false +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* Revert "feat(params): set use_sim_time default as True" + This reverts commit da85edf4956083563715daa3d60f0da1f94a423d. +* Merge branch 'master' into fix/use-sim-time-for-real-time-factor-control +* feat(params): set use_sim_time default as True +* Merge remote-tracking branch 'origin/master' into fix/use-sim-time-for-real-time-factor-control +* Merge branch 'master' into doc/RJD-1273-add-realtime-factor-doc +* Contributors: Dawid Moszynski, Dawid Moszyński, Kotaro Yoshimoto + +4.1.0 (2024-09-03) +------------------ +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-line-segment +* Merge branch 'master' into RJD-1278/fix-1344-getIntersection2DSValue +* Merge branch 'master' into RJD-1278/fix-1343-isIntersect2D +* Contributors: Michał Ciasnocha + +4.0.4 (2024-09-02) +------------------ +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Merge branch 'master' into feature/simple_sensor_simulator_unit_tests_lidar +* Contributors: Masaya Kataoka, SzymonParapura + +4.0.3 (2024-08-29) +------------------ +* Merge remote-tracking branch 'origin/master' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'tier4/RJD-1056-remove-current-time-step-time' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/RJD-1056-remove-npc-logic-started' into RJD-1057-base +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-npc-logic-started +* Contributors: DMoszynski, Dawid Moszynski, Mateusz Palczuk + +4.0.2 (2024-08-28) +------------------ +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'master' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge remote-tracking branch 'origin/ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' into RJD-1056-remove-current-time-step-time +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + +4.0.1 (2024-08-28) +------------------ +* Merge branch 'master' into fix/follow_trajectory +* Merge branch 'master' into fix/follow_trajectory +* Merge remote-tracking branch 'origin' into fix/follow_trajectory +* Contributors: Masaya Kataoka + +4.0.0 (2024-08-27) +------------------ +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'master' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-set-update-canonicalized-entity-status' of https://github.com/tier4/scenario_simulator_v2 into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge branch 'ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Merge remote-tracking branch 'origin/ref/RJD-1053-implement-pose-utils' into ref/RJD-1053-set-update-canonicalized-entity-status +* Contributors: DMoszynski, Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk, Tatsuya Yamasaki + +3.5.5 (2024-08-27) +------------------ +* Merge pull request `#1348 `_ from tier4/fix/distance-with-lane-change + Fix longitudinal dintance calculation with lane-change in `HdMapUtils::getLongitudinalDistance` +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* Merge branch 'master' into fix/distance-with-lane-change +* chore: add corner case to fix into a scenario +* Contributors: Kotaro Yoshimoto, Masaya Kataoka + +3.5.4 (2024-08-26) +------------------ +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge branch 'master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/use_workflow_dispatch_in_docker_build +* Merge remote-tracking branch 'origin/master' into feature/trigger_docker_build_by_tag +* Contributors: Masaya Kataoka + +3.5.3 (2024-08-26) +------------------ +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Merge branch 'master' into RJD-1278/traffic_simulator-update +* Contributors: Michał Ciasnocha + +3.5.2 (2024-08-23) +------------------ +* Merge pull request `#1338 `_ from tier4/fix/interpreter/user-defined-value-condition + Fix/interpreter/user defined value condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge branch 'master' into fix/interpreter/user-defined-value-condition +* Merge remote-tracking branch 'origin/master' into fix/interpreter/user-defined-value-condition +* Update test scenario `ByValueCondition.UserDefinedValueCondition.yaml` +* Update test scenario `ByValueCondition.UserDefinedValueCondition.yaml` +* Contributors: Tatsuya Yamasaki, yamacir-kit + +3.5.1 (2024-08-22) +------------------ +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Merge branch 'master' into feat/RJD-1283-add-traffic-controller-visualization +* Contributors: Dawid Moszyński, Tatsuya Yamasaki + +3.5.0 (2024-08-21) +------------------ +* Merge pull request `#1316 `_ from tier4/relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* feat: add test scenarios for RelativeClearanceCondition to CI test +* Merge branch 'master' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Add ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml +* Update ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml to be a better test +* Update ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml to check RelativeClearanceCondition in more detail +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Merge remote-tracking branch 'origin/relative-clearance-condition' into relative-clearance-condition +* Merge branch 'master' into relative-clearance-condition +* feat: add ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml +* Merge remote-tracking branch 'origin/master' into relative-clearance-condition +* Contributors: Kotaro Yoshimoto, Tatsuya Yamasaki + +3.4.4 (2024-08-20) +------------------ + +3.4.3 (2024-08-19) +------------------ + +3.4.2 (2024-08-05) +------------------ +* Merge commit 'c1cab6eb1ece2df58982f50a78fef5a5ecaa7234' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Merge branch 'master' into doc/longitudinal-control +* Merge branch 'master' into feat/RJD-1199-add-imu-sensor-to-simple-sensor-simulator +* Contributors: Masaya Kataoka, SzymonParapura, koki suzuki + +3.4.1 (2024-07-30) +------------------ +* Merge branch 'master' into doc/open_scenario_support +* Contributors: Tatsuya Yamasaki + +3.4.0 (2024-07-26) +------------------ +* Merge pull request `#1325 `_ from tier4/feature/interpreter/lidar-configuration + Feature/interpreter/lidar configuration +* Add a test scenario for `ObjectController`'s pseudo LiDAR property +* Contributors: Masaya Kataoka, yamacir-kit + 3.3.0 (2024-07-23) ------------------ * Merge pull request `#1059 `_ from tier4/feature/interpreter/entity_selection diff --git a/test_runner/scenario_test_runner/config/workflow.txt b/test_runner/scenario_test_runner/config/workflow.txt index 632b7495ced..06d459988cc 100644 --- a/test_runner/scenario_test_runner/config/workflow.txt +++ b/test_runner/scenario_test_runner/config/workflow.txt @@ -4,6 +4,8 @@ $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityConditio $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml $(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeDistanceCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml $(find-pkg-share scenario_test_runner)/scenario/ControllerAction.AssignControllerAction.yaml $(find-pkg-share scenario_test_runner)/scenario/LongitudinalAction.SpeedAction.yaml $(find-pkg-share scenario_test_runner)/scenario/Property.isBlind.yaml diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 4d0b80e55f3..6ebccaa9c89 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -58,6 +58,10 @@ def default_autoware_launch_file_of(architecture_type): }[architecture_type] +def default_rviz_config_file(): + return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz" + + def launch_setup(context, *args, **kwargs): # fmt: off architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20230906") @@ -77,7 +81,7 @@ def launch_setup(context, *args, **kwargs): port = LaunchConfiguration("port", default=5555) publish_empty_context = LaunchConfiguration("publish_empty_context", default=False) record = LaunchConfiguration("record", default=True) - rviz_config = LaunchConfiguration("rviz_config", default="") + rviz_config = LaunchConfiguration("rviz_config", default=default_rviz_config_file()) scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) sensor_model = LaunchConfiguration("sensor_model", default="") sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) @@ -129,22 +133,29 @@ def make_parameters(): {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, {"sigterm_timeout": sigterm_timeout}, + {"use_sim_time": use_sim_time}, {"vehicle_model": vehicle_model}, ] - parameters += make_vehicle_parameters() - return parameters - def make_vehicle_parameters(): - parameters = [] + def collect_vehicle_parameters(): + if vehicle_model_name := vehicle_model.perform(context): + description = get_package_share_directory(vehicle_model_name + "_description") + return [ + description + "/config/vehicle_info.param.yaml", + description + "/config/simulator_model.param.yaml", + ] + else: + return [] + + if (it := collect_vehicle_parameters()) != []: + parameters += it - def description(): - return get_package_share_directory( - vehicle_model.perform(context) + "_description" - ) + def collect_prefixed_parameters(): + return [item[0][9:] + ':=' + item[1] for item in context.launch_configurations.items() if item[0][:9] == 'autoware.'] + + if (it := collect_prefixed_parameters()) != []: + parameters += [{"autoware.": it}] - if vehicle_model.perform(context): - parameters.append(description() + "/config/vehicle_info.param.yaml") - parameters.append(description() + "/config/simulator_model.param.yaml") return parameters return [ @@ -192,7 +203,7 @@ def description(): namespace="simulation", output="screen", on_exit=ShutdownOnce(), - parameters=make_parameters() + [{"use_sim_time": True}], + parameters=make_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear. @@ -205,7 +216,7 @@ def description(): executable="openscenario_interpreter_node", namespace="simulation", output="screen", - parameters=[{"use_sim_time": use_sim_time}]+make_parameters(), + parameters=make_parameters(), prefix=make_launch_prefix(), on_exit=ShutdownOnce(), ), @@ -229,13 +240,7 @@ def description(): name="rviz2", output={"stderr": "log", "stdout": "log"}, condition=IfCondition(launch_rviz), - arguments=[ - "-d", - str( - Path(get_package_share_directory("traffic_simulator")) - / "config/scenario_simulator_v2.rviz" - ), - ], + arguments=["-d", str(default_rviz_config_file())], ), ] diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 0e6b321b352..b165aa863f6 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 3.3.0 + 4.3.7 scenario test runner package Tatsuya Yamasaki Apache License 2.0 diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml index 712febf2313..68d0bea8762 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceCondition.Shortest.yaml @@ -92,6 +92,17 @@ OpenSCENARIO: offset: 0 Orientation: *DEFAULT_ORIENTATION - LongitudinalAction: *SPEED_ACTION_ZERO + - entityRef: car_4 + PrivateAction: + - TeleportAction: + Position: &POSITION_4 + LanePosition: + roadId: "" + laneId: 34795 + s: 5 + offset: 0 + Orientation: *DEFAULT_ORIENTATION + - LongitudinalAction: *SPEED_ACTION_ZERO Story: - name: story Act: @@ -582,6 +593,24 @@ OpenSCENARIO: routingAlgorithm: shortest rule: notEqualTo value: 1.262040583529198123358128214022 + - Condition: + - name: "corner case fixed in #1348" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: car_1 + EntityCondition: + RelativeDistanceCondition: + coordinateSystem: lane + entityRef: car_4 + freespace: false + relativeDistanceType: longitudinal + routingAlgorithm: shortest + rule: notEqualTo + value: 54.18867466433655977198213804513216018676757812500000 StartTrigger: ConditionGroup: - Condition: diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml new file mode 100644 index 00000000000..2d6975cf0e4 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition-back.yaml @@ -0,0 +1,262 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: "1970-01-01T09:00:00+09:00" + author: Kotaro Yoshimoto + description: "" + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(find-pkg-share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: &SAMPLE_VEHICLE + catalogName: sample_vehicle + entryName: sample_vehicle + - name: car_same_front + CatalogReference: *SAMPLE_VEHICLE + - name: car_right_front + CatalogReference: *SAMPLE_VEHICLE + - name: car_right_back + CatalogReference: *SAMPLE_VEHICLE + - name: car_same_back + CatalogReference: *SAMPLE_VEHICLE + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: &EGO_LANE_POSITION + roadId: "" + laneId: 34513 + s: 15 + offset: 0 + Orientation: + type: relative + h: 3.141592653589793 + p: 0 + r: 0 + - LongitudinalAction: &SPEED_ACTION_ZERO + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + - entityRef: car_same_front + PrivateAction: + - TeleportAction: + Position: + LanePosition: &CAR_1_LANE_POSITION + << : *EGO_LANE_POSITION + s: 25 + - LongitudinalAction: *SPEED_ACTION_ZERO + - entityRef: car_right_front + PrivateAction: + - TeleportAction: + Position: + LanePosition: &CAR_2_LANE_POSITION + << : *EGO_LANE_POSITION + laneId: 34462 + s: 25 + - LongitudinalAction: *SPEED_ACTION_ZERO + - entityRef: car_right_back + PrivateAction: + - TeleportAction: + Position: + LanePosition: + << : *EGO_LANE_POSITION + laneId: 34462 + s: 10 + - LongitudinalAction: *SPEED_ACTION_ZERO + - entityRef: car_same_back + PrivateAction: + - TeleportAction: + Position: + LanePosition: + << : *EGO_LANE_POSITION + s: 10 + - LongitudinalAction: *SPEED_ACTION_ZERO + Story: + - name: story + Act: + - name: act + ManeuverGroup: + - maximumExecutionCount: 1 + name: maneuver_group + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: maneuver + Event: + - name: failure + priority: parallel + Action: + - name: "" + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: "check all car is in range(not clear) means the condition is false" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + distanceBackward: 12 + distanceForward: 7 + freeSpace: false + oppositeLanes: false + - Condition: + - name: "check not all car is in range(not clear) means the condition is false" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + distanceBackward: 0 + distanceForward: 7 + freeSpace: false + oppositeLanes: false + - Condition: + - name: "timeout" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 1 + rule: greaterThan + - name: success + priority: parallel + Action: + - name: "" + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + StartTrigger: + ConditionGroup: + - Condition: + - name: "check clear with all cars in narrower longitudinal range" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + distanceBackward: 8 + distanceForward: 3 + freeSpace: false + oppositeLanes: false + - name: "check back is clear with front cars" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_same_front + - entityRef: car_right_front + distanceBackward: 0 + distanceForward: 7 + freeSpace: false + oppositeLanes: false + - name: "check front is clear with back cars" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_right_back + - entityRef: car_same_back + distanceBackward: 12 + distanceForward: 0 + freeSpace: false + oppositeLanes: false + - name: "check right is clear with cars in the same lane" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_same_front + - entityRef: car_same_back + distanceBackward: 12 + distanceForward: 7 + freeSpace: false + oppositeLanes: false + RelativeLaneRange: + - from: 1 + to: 1 + - name: "check same lane is clear with cars in the right lane" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_right_front + - entityRef: car_right_back + distanceBackward: 12 + distanceForward: 7 + freeSpace: false + oppositeLanes: false + RelativeLaneRange: + - from: 0 + to: 0 + StartTrigger: + ConditionGroup: + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: [] diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml new file mode 100644 index 00000000000..da97625a8af --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeClearanceCondition.yaml @@ -0,0 +1,262 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: "1970-01-01T09:00:00+09:00" + author: Kotaro Yoshimoto + description: "" + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(find-pkg-share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: &SAMPLE_VEHICLE + catalogName: sample_vehicle + entryName: sample_vehicle + - name: car_same_front + CatalogReference: *SAMPLE_VEHICLE + - name: car_right_front + CatalogReference: *SAMPLE_VEHICLE + - name: car_right_back + CatalogReference: *SAMPLE_VEHICLE + - name: car_same_back + CatalogReference: *SAMPLE_VEHICLE + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: &EGO_LANE_POSITION + roadId: "" + laneId: 34513 + s: 15 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: &SPEED_ACTION_ZERO + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + - entityRef: car_same_front + PrivateAction: + - TeleportAction: + Position: + LanePosition: &CAR_1_LANE_POSITION + << : *EGO_LANE_POSITION + s: 25 + - LongitudinalAction: *SPEED_ACTION_ZERO + - entityRef: car_right_front + PrivateAction: + - TeleportAction: + Position: + LanePosition: &CAR_2_LANE_POSITION + << : *EGO_LANE_POSITION + laneId: 34462 + s: 25 + - LongitudinalAction: *SPEED_ACTION_ZERO + - entityRef: car_right_back + PrivateAction: + - TeleportAction: + Position: + LanePosition: + << : *EGO_LANE_POSITION + laneId: 34462 + s: 10 + - LongitudinalAction: *SPEED_ACTION_ZERO + - entityRef: car_same_back + PrivateAction: + - TeleportAction: + Position: + LanePosition: + << : *EGO_LANE_POSITION + s: 10 + - LongitudinalAction: *SPEED_ACTION_ZERO + Story: + - name: story + Act: + - name: act + ManeuverGroup: + - maximumExecutionCount: 1 + name: maneuver_group + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: maneuver + Event: + - name: failure + priority: parallel + Action: + - name: "" + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: "check all car is in range(not clear) means the condition is false" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + distanceBackward: 7 + distanceForward: 12 + freeSpace: false + oppositeLanes: false + - Condition: + - name: "check not all car is in range(not clear) means the condition is false" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + distanceBackward: 7 + distanceForward: 0 + freeSpace: false + oppositeLanes: false + - Condition: + - name: "timeout" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 1 + rule: greaterThan + - name: success + priority: parallel + Action: + - name: "" + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + StartTrigger: + ConditionGroup: + - Condition: + - name: "check clear with all cars in narrower longitudinal range" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + distanceBackward: 3 + distanceForward: 8 + freeSpace: false + oppositeLanes: false + - name: "check back is clear with front cars" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_same_front + - entityRef: car_right_front + distanceBackward: 7 + distanceForward: 0 + freeSpace: false + oppositeLanes: false + - name: "check front is clear with back cars" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_right_back + - entityRef: car_same_back + distanceBackward: 0 + distanceForward: 12 + freeSpace: false + oppositeLanes: false + - name: "check right is clear with cars in the same lane" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_same_front + - entityRef: car_same_back + distanceBackward: 7 + distanceForward: 12 + freeSpace: false + oppositeLanes: false + RelativeLaneRange: + - from: -1 + to: -1 + - name: "check same lane is clear with cars in the right lane" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: + - entityRef: ego + EntityCondition: + RelativeClearanceCondition: + EntityRef: + - entityRef: car_right_front + - entityRef: car_right_back + distanceBackward: 7 + distanceForward: 12 + freeSpace: false + oppositeLanes: false + RelativeLaneRange: + - from: 0 + to: 0 + StartTrigger: + ConditionGroup: + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: [] diff --git a/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml b/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml index 0b340721977..427fcce95df 100644 --- a/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml +++ b/test_runner/scenario_test_runner/scenario/ByValueCondition.UserDefinedValueCondition.yaml @@ -8,7 +8,9 @@ OpenSCENARIO: ParameterDeclarations: ParameterDeclaration: [] CatalogLocations: - CatalogLocation: [] + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle RoadNetwork: LogicFile: filepath: $(find-pkg-share kashiwanoha_map)/map @@ -17,45 +19,32 @@ OpenSCENARIO: Entities: ScenarioObject: - name: ego - Vehicle: + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: + - name: isEgo + value: 'true' + - name: barricade + MiscObject: + mass: 1.0 + miscObjectCategory: obstacle name: '' - vehicleCategory: car BoundingBox: Center: x: 0 y: 0 - z: 1.25 + z: 0 Dimensions: - width: 2.25 - length: 4.77 - height: 2.5 - Performance: - maxSpeed: 50 - maxAcceleration: INF - maxDeceleration: INF - Axles: - FrontAxle: - maxSteering: 0.5236 - wheelDiameter: 0.6 - trackWidth: 4 - positionX: 1 - positionZ: 0.3 - RearAxle: - maxSteering: 0.5236 - wheelDiameter: 0.6 - trackWidth: 4 - positionX: 0 - positionZ: 0.3 + width: 10 + length: 1 + height: 10 Properties: Property: [] - ObjectController: - Controller: - name: '' - Properties: - Property: - - { name: isEgo, value: 'true' } - - { name: maxJerk, value: 1.5 } - - { name: minJerk, value: -1.5 } Storyboard: Init: Actions: @@ -69,33 +58,30 @@ OpenSCENARIO: laneId: '34513' s: 0 offset: 0 - Orientation: + Orientation: &ORIENTATION_ZERO type: relative h: 0 p: 0 r: 0 - RoutingAction: AcquirePositionAction: - Position: + Position: &EGO_DESTINATION LanePosition: roadId: '' laneId: '34507' s: 50 offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 - - LongitudinalAction: - SpeedAction: - SpeedActionDynamics: - dynamicsDimension: time - value: 0 - dynamicsShape: step - SpeedActionTarget: - AbsoluteTargetSpeed: - value: 2.778 + Orientation: *ORIENTATION_ZERO + - entityRef: barricade + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: 34513 + s: 20 + offset: 0 + Orientation: *ORIENTATION_ZERO Story: - name: '' Act: @@ -120,7 +106,7 @@ OpenSCENARIO: conditionEdge: none ByValueCondition: SimulationTimeCondition: - value: 120 + value: 180 rule: greaterThan - Condition: - name: '' @@ -133,26 +119,8 @@ OpenSCENARIO: - entityRef: ego EntityCondition: ReachPositionCondition: - Position: - LanePosition: - roadId: '' - laneId: '34507' - s: 50 - offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 + Position: *EGO_DESTINATION tolerance: 0.5 - - name: '' - delay: 0 - conditionEdge: none - ByValueCondition: - UserDefinedValueCondition: - name: ego.currentState - rule: equalTo - value: ArrivedGoal Action: - name: '' UserDefinedAction: @@ -163,23 +131,178 @@ OpenSCENARIO: StartTrigger: ConditionGroup: - Condition: - - name: '' + - &COUNT_UP + name: '' delay: 0 conditionEdge: none ByValueCondition: UserDefinedValueCondition: - name: /count_up - value: 500 + name: /count_up # ros2 run openscenario_interpreter_example count_up + value: 100 rule: greaterThan - # - Condition: - - name: '' - delay: 0 - conditionEdge: none - ByValueCondition: - UserDefinedValueCondition: - name: /timeout - value: true - rule: equalTo + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 10 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 20 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 30 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 40 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 50 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 60 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 70 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 80 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 90 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 100 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 110 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 120 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 130 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 140 + + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP + - *COUNT_UP # 150 Action: - name: '' UserDefinedAction: diff --git a/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml b/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml new file mode 100644 index 00000000000..ffadea00e1a --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/Property.pointcloudVerticalFieldOfView.yaml @@ -0,0 +1,259 @@ +OpenSCENARIO: + FileHeader: + author: 'Tatsuya Yamasaki' + date: '2022-03-04T18:06:53+09:00' + description: 'Sample scenario (with Autoware)' + revMajor: 1 + revMinor: 0 + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: 'Autoware' + Properties: + Property: + - name: maxJerk + value: "1.5" + - name: minJerk + value: "-1.5" + - name: pointcloudChannels + value: '67' + - name: pointcloudHorizontalResolution + value: '1.5' + - name: pointcloudVerticalFieldOfView + value: '45.678' + - name: boundary + MiscObject: &BARRICADE + mass: 1.0 + miscObjectCategory: obstacle + name: '' + BoundingBox: + Center: + x: 0 + y: 0 + z: 0 + Dimensions: + width: 100 + length: 100 + height: 100 + Properties: + Property: [] + - name: barricade + MiscObject: + mass: 1.0 + miscObjectCategory: obstacle + name: '' + BoundingBox: + Center: + x: 0 + y: 0 + z: 0 + Dimensions: + width: 10 + length: 1 + height: 10 + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: 34513 + s: 10 + offset: 0 + Orientation: &ORIENTATION_ZERO + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: + LanePosition: + roadId: '' + laneId: '34507' + s: 50 + offset: 0 + Orientation: *ORIENTATION_ZERO + - entityRef: boundary + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: 34513 + s: 10 + offset: 0 + Orientation: *ORIENTATION_ZERO + - entityRef: barricade + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: 34513 + s: 20 + offset: 0 + Orientation: *ORIENTATION_ZERO + Story: + - name: '' + Act: + - name: remove_barricade + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: barricade + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + maximumExecutionCount: 1 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: barricade + EntityCondition: + StandStillCondition: + duration: 90 + Action: + - name: '' + GlobalAction: + EntityAction: + entityRef: barricade + DeleteEntityAction: + StartTrigger: + ConditionGroup: + - Condition: + - name: + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: + LanePosition: + roadId: '' + laneId: '34507' + s: 50 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + tolerance: 0.5 + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + UserDefinedValueCondition: + name: RelativeHeadingCondition(ego, 34507, 50) + rule: lessThan + value: 0.1 + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + UserDefinedValueCondition: + name: RelativeHeadingCondition(ego) + rule: lessThan + value: 0.1 + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + UserDefinedValueCondition: + name: ego.currentMinimumRiskManeuverState.state + rule: equalTo + value: NORMAL + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 180 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: [] diff --git a/test_runner/scenario_test_runner/scenario/RoutingAction.AcquirePositionAction-continuous.yaml b/test_runner/scenario_test_runner/scenario/RoutingAction.AcquirePositionAction-continuous.yaml new file mode 100644 index 00000000000..db959904eb2 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/RoutingAction.AcquirePositionAction-continuous.yaml @@ -0,0 +1,290 @@ +OpenSCENARIO: + FileHeader: + author: 'Tatsuya Yamasaki' + date: '1970-01-01T09:00:00+09:00' + description: '' + revMajor: 1 + revMinor: 0 + ParameterDeclarations: + ParameterDeclaration: + - name: laps + parameterType: unsignedInt + value: 3 + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: 'Autoware' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: &INITIAL_POSITION + LanePosition: + roadId: '' + laneId: '34513' + s: 1 + offset: 0 + Orientation: &DEFAULT_ORIENTATION + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: &POSITION_A + LanePosition: + roadId: '' + laneId: '34681' + s: 1 + offset: 0 + Orientation: *DEFAULT_ORIENTATION + Story: + - name: '' + Act: + - name: loop + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + ParameterDeclarations: + ParameterDeclaration: + - name: current_destination + parameterType: string + value: position_a + Event: + - maximumExecutionCount: $laps + name: '' + priority: parallel + Action: + - name: '' + PrivateAction: + - RoutingAction: + AcquirePositionAction: + Position: *POSITION_A + - name: '' + GlobalAction: + ParameterAction: + parameterRef: current_destination + SetAction: + value: position_a + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: &POSITION_B + LanePosition: + roadId: '' + laneId: '34408' + s: 1 + offset: 0 + Orientation: *DEFAULT_ORIENTATION + tolerance: 1 + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + SpeedCondition: + rule: equalTo + value: 0 + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + UserDefinedValueCondition: + name: ego.currentState + rule: equalTo + value: WAITING_FOR_ROUTE + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + ParameterCondition: + parameterRef: current_destination + rule: notEqualTo + value: position_a + - maximumExecutionCount: $laps + name: '' + priority: parallel + Action: + - name: '' + PrivateAction: + - RoutingAction: + AcquirePositionAction: + Position: *POSITION_B + - name: '' + GlobalAction: + ParameterAction: + parameterRef: current_destination + SetAction: + value: position_b + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *POSITION_A + tolerance: 1 + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + SpeedCondition: + rule: equalTo + value: 0 + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + UserDefinedValueCondition: + name: ego.currentState + rule: equalTo + value: WAITING_FOR_ROUTE + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + ParameterCondition: + parameterRef: current_destination + rule: notEqualTo + value: position_b + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: loop + storyboardElementType: act + state: completeState + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *POSITION_A + tolerance: 1 + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: ${90 + $laps * 120} + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + StandStillCondition: + duration: 10 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: []