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..b9638744476 100644 --- a/.github/workflows/custom_spell.json +++ b/.github/workflows/custom_spell.json @@ -6,6 +6,7 @@ "Canonicalized", "canonicalizing", "classname", + "Cmyk", "Dawid", "DBOOST", "DBUILD", @@ -40,6 +41,7 @@ "TESTRANDOMIZER", "travelling", "Tschirnhaus", + "walltime", "xerces", "xercesc", "yamacir-kit" diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 36cb7013215..5a2fb5ef910 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,232 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 92e99d91604..e717cb6ffe5 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 3.2.0 + 4.2.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 bb028c0cecf..960205286db 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,289 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose 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 e27cfc406b8..595b28a4815 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 3.2.0 + 4.2.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 77184cb6768..d5374c143d1 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,233 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 328e3a7fc6e..be1bc7047ea 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 3.2.0 + 4.2.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 fd10304d3a1..798f39ad289 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,234 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 50997d2ee08..230fc8fa2a4 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 3.2.0 + 4.2.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 09d1d8cb6fc..3777997de87 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,233 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 50bbf0e089b..e6240847dd5 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 3.2.0 + 4.2.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/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md b/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md index 6a721ea6fef..b7c7ae97ae7 100644 --- a/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md +++ b/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md @@ -1,7 +1,7 @@ # Lane pose calculation when getting longitudinal distance Attempts to calculate the pose for adjacent lane coordinate systems when measuring longitudinal distance. -The length of the horizontal bar must intersect with the adjacent lanelet, [so it is always 10 m regardless of the entity type.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/entity_manager.cpp#L375C21-L442) +The length of the horizontal bar must intersect with the adjacent lanelet, [so it is always 10 m regardless of the entity type.](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122) ![Get longitudinal distance](../../image/longitudinal_distance.png "Getting longitudinal distance.") diff --git a/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md b/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md index b00317b8735..445e789dc18 100644 --- a/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md +++ b/docs/developer_guide/lane_pose_calculation/LanePoseCalculation.md @@ -12,19 +12,23 @@ These are the [timing immediately after the spawn of the Entity](Spawn.md), the ## Lane coordinate system calculation algorithm for a specific lane !!! summary - - The calculation of the pose in the lane coordinate system involves two steps: first, filtering by "which lanes can be matched," and then "calculating the pose in the specific lane coordinate system. + - The calculation of the pose in the lane coordinate system involves two steps: first, filtering by "which lanes can be matched," and then "calculating the pose in the specific lane coordinate system". - The parameter that most affects the matching results is the length of the horizontal bar. This length depends on the type of entity and the timing of the calculation. If you want to check the length quickly, please check [this section](#quick-guide-to-horizontal-bar-lengths). ### Quick guide to horizontal bar lengths The unit of the table is meter. -| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md) | [Getting longitudinal distance](GetLongitudinalDistance.md) | -| ---------------------------------- | --------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------- | ----------------------------------------------------------- | -| EgoEntity | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | [(tread of the entity) + 2.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) | 10 | -| VehicleEntity | [(tread of the entity) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | N/A | 10 | -| PedestrianEntity | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | [2.0 or 4.0](UpdateFrame.md#pedestrian-entity-with-behavior-tree) | 10 | -| MiscObject | [(width of the entity bounding box) + 2.0](Spawn.md#calculate-pose-in-lane-coordinate-system) | N/A | 10 | +Tread of the Entity is the maximum value of the tread from its front and rear wheels divided by two. +Tread of the front wheels is $t_f$ and the tread of the rear wheels is $t_r$. +$tread = \frac{max(t_f, t_r)}{2}$ + +| EntityType \ Timing of calculation | [Spawn](Spawn.md) | [UpdateFrame](UpdateFrame.md) | [Getting longitudinal distance](GetLongitudinalDistance.md) | +| ---------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| EgoEntity | [(tread of the entity) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | [(tread of the entity) + 1.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/vehicle_entity.cpp#L78-L85), [code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp#L453-L456)) | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | +| VehicleEntity | [(tread of the entity) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | [(tread of the entity) + 1.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/vehicle_entity.cpp#L78-L85)) | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | +| PedestrianEntity | [(width of the entity bounding box) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | [(width of the entity bounding box) + 1.0](UpdateFrame.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/entity_base.cpp#L86-L89)) | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | +| MiscObject | [(width of the entity bounding box) + 1.0](Spawn.md#calculate-pose-in-lane-coordinate-system) ([code](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L458-L468)) | N/A | 10 ([code](https://github.com/tier4/scenario_simulator_v2/blob/f577103d685523a3e8e19fd72c9a1c3c2d032704/simulation/traffic_simulator/src/utils/distance.cpp#L48-L122)) | ### Detail of the lane coordinate system calculation algorithm for a specific lane diff --git a/docs/developer_guide/lane_pose_calculation/Spawn.md b/docs/developer_guide/lane_pose_calculation/Spawn.md index a78890e3fad..7531148f5dc 100644 --- a/docs/developer_guide/lane_pose_calculation/Spawn.md +++ b/docs/developer_guide/lane_pose_calculation/Spawn.md @@ -1,15 +1,15 @@ # Lane pose calculation when spawning -If you spawn entities by specifying in the lane coordinate system, lane pose calculation is skipped. +If you spawn entities by specifying in the lane coordinate system, [lane pose calculation is skipped](https://github.com/tier4/scenario_simulator_v2/blob/0ddeacd1eab22ffc334202ff9a5c458b5569dd32/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L473-L485). Internally, spawning entities by specifying in the lane coordinate system means specifying LaneletPose as the argument pose of the [API::spawn function](https://tier4.github.io/scenario_simulator_v2-api-docs/classtraffic__simulator_1_1API.html). In the OpenSCENARIO, it is to execute a TeleportAction by specifying the LanePosition. ([e.g](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/test_runner/scenario_test_runner/)) -If you spawn entities by specifying in the map coordinate system, [lane pose calculation](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L521-L529) is performed. +If you spawn entities by specifying in the map coordinate system, [lane pose calculation is performed](https://github.com/tier4/scenario_simulator_v2/blob/0ddeacd1eab22ffc334202ff9a5c458b5569dd32/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp#L473-L485). There are several internal implementations of pose calculation in the lane coordinate system implemented in the [HDmapUtils class](https://tier4.github.io/scenario_simulator_v2-api-docs/classhdmap__utils_1_1HdMapUtils.html). The one used in this case is [here](https://github.com/tier4/scenario_simulator_v2/blob/9f03394f80e5de05cf087db3d00c7be73d27e963/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp#L602-L630). -In this function, the following procedure is used to calculate the pose in the lane coordinate system. +[This function](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L101-L122) and its overloads are commonly used to convert map pose to the lane coordinate system. ## Search for matching lanes @@ -34,10 +34,10 @@ After narrowing down the candidate lanes by the above procedure, the pose of the Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and the tread of the front wheels be $t_f$ and the tread of the rear wheels be $t_r$. -$$L_m = max(t_r, t_f) + 2.0$$ +$$L_m = max(t_r, t_f) + 1.0$$ ### Pedestrian / MiscObject Entity Let $L_m$ be the length of the horizontal bar used in the lane coordinate system calculation and width of the bounding box be $L_w$ -$$L_m = L_w + 2.0$$ +$$L_m = L_w + 1.0$$ diff --git a/docs/developer_guide/lane_pose_calculation/UpdateFrame.md b/docs/developer_guide/lane_pose_calculation/UpdateFrame.md index bc201cc8d2f..09098fb3df8 100644 --- a/docs/developer_guide/lane_pose_calculation/UpdateFrame.md +++ b/docs/developer_guide/lane_pose_calculation/UpdateFrame.md @@ -5,16 +5,14 @@ If the entity's behavioral logic is planned in the lane coordinate system, skip ## Ego Entity Since EgoEntity is controlled in map coordinates by Autoware rather than in lane coordinates using a motion plugin, this process is performed at each frame update. -This process is implemented [here](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/api/api.cpp#L240C9-L240C19) and [here](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L276-L312). +This process is implemented [API](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/api/api.cpp#L302) -> [EgoEntity::setMapPose](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/ego_entity.cpp#L294-L300) -> [EgoEntity::setStatus](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/ego_entity.cpp#L309-L320). ### Search for matching lanes -This process branches off into 2 or 3 depending on the outcome of 1. -If 3 is executed and fails, fallbacks to 4. +This process branches off into 2. -1. [Obtain a candidate lane for matching from Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L276) -2. [If Autoware has no planner output, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L286-L287) -3. [If Autoware has planner output, try matching considering the Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L289-L290) -4. [If the step 3 failed, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/entity/ego_entity.cpp#L292-L293) +1. [Obtain a candidate lane for matching from Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/ego_entity.cpp#L296) +2. [If Autoware has planner output, try matching considering the Autoware planner output.](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L110-L111) +3. [If Autoware has no planner output or 2 fails, try obtaining candidate lanes for matching considering the bounding box.](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L114-L115) ### Calculate pose in lane coordinate system @@ -27,12 +25,14 @@ $$L_m = max(t_r, t_f) + 2.0$$ ## Non-Ego Entity -For non-EgoEntity, the process diverges depending on whether planning is done in the lane coordinate system inside BehaviorPlugin. -If planning is done in the lane coordinate system, there is no need to convert from the map coordinate system to the lane coordinate system after update frame function is executed. +For non-Ego Entities, the pose calculation process is carried out immediately after getting the updated EntityStatus from BehaviorTree. +If the planning is done in the lane coordinate system, the value of `lanelet_pose_valid` is equal to true, which causes [only position canonicalization](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/data_type/entity_status.cpp#L75-L76) to be performed. ### VehicleEntity (with Behavior-Tree) -Only during `follow_polyline_trajectory` execution is planning performed in map coordinate system, but [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L546) +Only during `follow_polyline_trajectory` execution is planning performed in map coordinate system, but [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L561) + +This process is implemented [VehicleEntity::onUpdate](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/vehicle_entity.cpp#L164-L165) -> [EntityBase::setStatus](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/entity/entity_base.cpp#L520-L523) -> [CanonicalizedEntityStatus::set](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/data_type/entity_status.cpp#L66-L84). ### Vehicle Entity (with Do-Nothing) @@ -42,21 +42,22 @@ While the do-nothing behavior plugin is running, Entity does not move, so the la Planning is done in map coordinates in the `walk_straight` and `follow_polyline_trajectory` actions. -In the `walk_straight` Action, [the pose in the lane coordinate system is calculated.](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L56) +In the `walk_straight` Action, [the pose in the lane coordinate system is calculated.](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L59-L62) The procedure for calculating the pose in the lane coordinate system at this time is as follows. -If lane matching was successful in the previous frame, do 1, otherwise do 2 +If lane matching was successful in the previous frame (entity is currently on some unique lane), [do 1, otherwise do 2](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L293-L297): -1.[Set the length of the horizontal bar to 2.0 + the width of the bounding box and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L72-L77) -2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/a2c04ee2446f80aeacfe59fc87a6737ae18692cc/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L79-L85) +1.[Set the length of the horizontal bar (to the width of the bounding box + 1.0) and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L110-L111) +2.[Calculate the pose in the lane coordinate system considering the size of the BoundingBox of the Entity](https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/simulation/traffic_simulator/src/utils/pose.cpp#L114-L115) If calculation 1 or 2 fails, -3.[Set the length of the horizontal bar to 4.0 and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L86-L91) +3.[Set the length of the horizontal bar to **2.0** and calculate the pose in the lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L302-L303) If 1 or 2 are successful, then 3 is skipped. -If the pose could not be calculated in the lane coordinate system by considering up to the result of 3, [the pose calculation in the lane coordinate system is a failure](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L125). +If the pose could not be calculated in the lane coordinate system by considering up to the result of 3, [the pose calculation in the lane coordinate system is a failure](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L335). -[Canonicalize pose in lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp#L94) to determine the final pose in the lane coordinate system. +[Canonicalize pose in lane coordinate system](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L303) to determine the final pose in the lane coordinate system. +If final canonicalize failed, [set end of road lanelet pose](https://github.com/tier4/scenario_simulator_v2/blob/6f87603e2aaaddfd8ceb08645cb3933fe3b74515/simulation/traffic_simulator/src/utils/pose.cpp#L302-L332). In the `follow_polyline_trajectory` Action, [lane coordinate system calculations are not performed](https://github.com/tier4/scenario_simulator_v2/blob/5f19d39ef29243396f26225976975f0c27914c12/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L546) 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/user_guide/QuickStart.md b/docs/user_guide/QuickStart.md index 1203f12df49..56bb42778bf 100644 --- a/docs/user_guide/QuickStart.md +++ b/docs/user_guide/QuickStart.md @@ -65,7 +65,7 @@ This document contains step-by-step instruction on how to build and run [AWF Aut #### 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 \ @@ -75,7 +75,7 @@ This document contains step-by-step instruction on how to build and run [AWF Aut #### 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 ``` 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 f41a2996a00..dfe7871201d 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,243 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose 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..0202d488eb3 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 @@ -114,17 +114,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/package.xml b/external/concealer/package.xml index 470eef34b69..3978cf815d1 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 3.2.0 + 4.2.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/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index 4637189d612..fa29fef0a6b 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,226 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index b0fa667371c..d46a9f5dc58 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 3.2.0 + 4.2.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 0e0e34cdf77..e2d80251f69 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,232 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 3edd9cbab40..58e76215a4e 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 3.2.0 + 4.2.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 669357e343c..21bad9eac16 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,198 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Contributors: Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index d46cd1a003f..7bff8b394b8 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 3.2.0 + 4.2.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 bdf221a0680..8a5f8d6fe76 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,262 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge pull request `#1323 `_ from tier4/fix/spawn_position_of_map_pose 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 5989f077922..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 @@ -52,6 +52,10 @@ class CppScenarioNode : public rclcpp::Node const std::vector & goal_lanelet_pose, const traffic_simulator_msgs::msg::VehicleParameters & parameters); + auto isVehicle(const std::string & name) const -> bool; + + auto isPedestrian(const std::string & name) const -> bool; + protected: traffic_simulator::API api_; common::junit::JUnit5 junit_; @@ -64,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 97e44351e3d..077636374e3 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 3.2.0 + 4.2.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 dff52151963..7864980bdab 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) @@ -131,6 +135,18 @@ void CppScenarioNode::spawnEgoEntity( } } +auto CppScenarioNode::isVehicle(const std::string & name) const -> bool +{ + return api_.getEntity(name)->getEntityType().type == + traffic_simulator_msgs::msg::EntityType::VEHICLE; +} + +auto CppScenarioNode::isPedestrian(const std::string & name) const -> bool +{ + return api_.getEntity(name)->getEntityType().type == + traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; +} + void CppScenarioNode::checkConfiguration(const traffic_simulator::Configuration & configuration) { try { diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 30f552af916..e1369f81976 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -138,9 +138,8 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } /// Checking the ego entity overs the lane change position. if (const auto entity = api_.getEntity("ego"); entity->laneMatchingSucceed()) { - if ( - entity->getStatus().getLaneletId() == 34684 && - std::abs(entity->getStatus().getLaneletPose().s) >= lane_change_position) { + const auto lanelet_pose = entity->getCanonicalizedStatus().getLaneletPose(); + if (lanelet_pose.lanelet_id == 34684 && std::abs(lanelet_pose.s) >= lane_change_position) { api_.requestLaneChange(entity_name, traffic_simulator::lane_change::Direction::RIGHT); lane_change_requested = true; } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index 6028842df1c..46532152560 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -75,19 +75,14 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 6fff5eeb1c5..7a5c8d97a83 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -52,19 +52,14 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 735e09b09e7..6d7976cfa05 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -52,15 +52,10 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); - - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - if (!entity_status.laneMatchingSucceed() || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (const auto entity = api_.getEntity(name)) { + if (!entity->laneMatchingSucceed() || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index 5c11cf4e960..4f9141d9d92 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -53,28 +53,20 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode } unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + if (isVehicle(name)) { + ++vehicle_count; + } else if (isPedestrian(name)) { + ++pedestrian_count; + } - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (is_vehicle) { - ++vehicle_count; - } else if (is_pedestrian) { - ++pedestrian_count; - } - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } if (vehicle_count == 0u || pedestrian_count == 0u) { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index 0cab69203d9..f4ba78518ff 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -53,37 +53,29 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode } unsigned int vehicle_count = 0u, pedestrian_count = 0u; for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; + const bool valid_pedestrian_lanelet = + api_.isInLanelet(name, static_cast(34385), 10.0); - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); + if (isVehicle(name)) { + ++vehicle_count; + } else if (isPedestrian(name)) { + ++pedestrian_count; + } - const bool valid_pedestrian_lanelet = - api_.isInLanelet(name, static_cast(34385), 10.0); - - if (is_vehicle) { - ++vehicle_count; - } else if (is_pedestrian) { - ++pedestrian_count; - } - - if ( - // clang-format off - !entity_status.laneMatchingSucceed() || - (is_vehicle && !valid_vehicle_lanelet) || - (is_pedestrian && !valid_pedestrian_lanelet)) - // clang-format on - { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if ( + // clang-format off + !entity->laneMatchingSucceed() || + (isVehicle(name) && !valid_vehicle_lanelet) || + (isPedestrian(name) && !valid_pedestrian_lanelet)) + // clang-format on + { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } if (vehicle_count != 6u || pedestrian_count != 15u) { diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index 9881f6b3e31..02db2b0bd26 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -52,18 +52,13 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); - - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - - if (entity_status.laneMatchingSucceed() || !is_pedestrian) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (const auto entity = api_.getEntity(name)) { + if (entity->laneMatchingSucceed() || !isPedestrian(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } + stop(cpp_mock_scenarios::Result::SUCCESS); } - stop(cpp_mock_scenarios::Result::SUCCESS); } } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index b5a31ecb211..02c46bbd755 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -52,18 +52,13 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_pedestrian_lanelet = + api_.isInLanelet(name, static_cast(34385), 10.0); - const bool is_pedestrian = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - - const bool valid_pedestrian_lanelet = - api_.isInLanelet(name, static_cast(34385), 10.0); - - if (!entity_status.laneMatchingSucceed() || !valid_pedestrian_lanelet || !is_pedestrian) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_pedestrian_lanelet || !isPedestrian(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index 699eff9d688..d38b251004f 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -52,19 +52,14 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE } for (const auto & name : names) { - const traffic_simulator::CanonicalizedEntityStatus entity_status = - api_.getEntityStatus(name); + if (const auto entity = api_.getEntity(name)) { + const bool valid_vehicle_lanelet = + api_.isInLanelet(name, static_cast(34705), 50.0) || + api_.isInLanelet(name, static_cast(34696), 50.0); - const bool is_vehicle = - static_cast(entity_status).type.type == - traffic_simulator_msgs::msg::EntityType::VEHICLE; - - const bool valid_vehicle_lanelet = - api_.isInLanelet(name, static_cast(34705), 50.0) || - api_.isInLanelet(name, static_cast(34696), 50.0); - - if (!entity_status.laneMatchingSucceed() || !valid_vehicle_lanelet || !is_vehicle) { - stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + if (!entity->laneMatchingSucceed() || !valid_vehicle_lanelet || !isVehicle(name)) { + stop(cpp_mock_scenarios::Result::FAILURE); // LCOV_EXCL_LINE + } } } stop(cpp_mock_scenarios::Result::SUCCESS); diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 7162e8708fe..b5e1570e9b8 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,232 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index a449aa595c1..b0597456e2b 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 3.2.0 + 4.2.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 ef31f377dad..85f8304ba99 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,446 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +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 + Add `EntitySelection` +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Add operator overloading for ostream +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Rename +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Remove implementation-defined types +* Unite `GroupedEntity` and `SingleEntity` into `Entity` +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Fix wrong branch +* Update entity lookup to consider empty name +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Format +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Fix constraint check +* Update assign `this` after parsing ScenarioObject +* Remove extra this +* Update constructor to pass args by reference +* Remove unneccessary type argument +* Remove unneccessary argument name +* Remove unneccessary argument name +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Add missing `return` +* Simplify entity inspection +* Simplify entity inspection +* Simplify entity inspection +* Simplify entity inspection +* Simplify entity inspection +* Update entity lookup +* Use universal reference +* Remove std::any_of +* Simplify for loop +* Rename +* Simplify `AccelerationCondition::evaluate` +* Format +* Update comparators to handle `std::valarray` +* Add `evaluate` to `GroupedEntity` +* Rename variable +* Use variadic function +* Rename argument +* Add this to lambda capture +* Remove extra header +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Revert changes on Entities +* Revert changes on Entities +* Revert changes on Entities +* Revert changes on Entities +* Fix wrong cast +* Format +* Fix headers and declarations +* Update entity implementation to distinguish plurality +* Format +* [WIP] Replace Entities::objects with Entity::objects +* Remove extra +* Make entity hashable +* Remove extra include and declaration +* Update entity exploration +* Add static field of object type +* Remove extra header +* Rename +* Implement `Entity` +* Add const version of `apply` +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Remove +* Remove unnecessary empty check +* Format +* Reorder header +* Update parameter name +* Rename HasIterator to Iterable +* Update to use EntityRef instead of String +* Use forward declration +* Format +* Remove extra header include +* Replace String entity_ref to original EntityRef +* Format +* Update to use type trait +* Add type trait for iterator detection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Remove unneccesary static_cast +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Fix wrong include guard +* Fix typo +* Remove wrong `return` +* Refactor +* Fix wrong argument order +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Use `Entities::isAdded` +* Assign `this` first to avoid null dereference +* Format +* Add missing condition +* Update to fill `results` with NaN by default +* Update not to use temporal +* Use `set` instead of `list` to deduplicate entity +* Change to use inline variable instead of struct +* Simplify +* Fix wrong printing process +* Refactor +* Fix wrong condition +* Update to check ExternalObjectReference +* Refactor +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Update conditions to handle EntitySelection properly +* Update `print_to` to print nested sequences +* Remove redundant iteration +* Simplify +* Format +* Update `Entities::isAdded` to handle `EntitySelection` +* Mark constructor explicit +* Implement constraints on entities of actions +* Implement `objectTypes` +* Update `ByType` to be like `EntityRef` +* Reorder +* Add simple impl of `ExternalObjectReference` +* Remove redundant explicit +* Make construcotors explicit +* Make `entities` public +* Rename header +* Rename +* Fix wrong message +* Add constraints check for actions +* Format +* Use `is_also` instead of `is` +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Use switch for byType +* Update `TeleportAction` to handle `EntitySelection` +* Update `SpeedProfileAction` to handle `EntitySelection` +* Update `SpeedAction` to handle `EntitySelection` +* Use new enumeration in `SelectedEntities` +* Update `LaneChangeAction` to handle `EntitySelection` +* Update `EntityAction` to use new enumeration +* Update `CustomCommandAction` to handle `EntitySelection` +* Update `ControllerAction` to handle `EntitySelection` +* Update `AssignRouteAction` to handle `EntitySelection` +* Update `AcquirePositionAction` to handle `EntitySelection` +* Update `Triggering_entities` to use new enumration +* Update object enumeration +* Add `EntitySelection` support for `TriggeringEntities` +* Add support for `EntitySelection` in `EntityAction` +* Improve entity enumeration +* Add entity enumeration +* Make `EntityRef` more specific to parsing +* Add `readNameRef` +* Remove unused functions +* [WIP] implement tree flattening +* Use class member instead of inheritance +* Make reference to `EntitySelection` error by default +* Ignore `EntitySelection` on engagement +* Fix wrong parsing of `SelectedEntities` +* Format include +* `EntitySelection`の不完全な読み取り処理の追加 +* エンティティの列挙処理の追加 +* Make `EntityRef` more specific to parsing +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Add `readNameRef` +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Remove unused functions +* [WIP] implement tree flattening +* Use class member instead of inheritance +* Make reference to `EntitySelection` error by default +* Ignore `EntitySelection` on engagement +* Fix wrong parsing of `SelectedEntities` +* Format include +* `EntitySelection`の不完全な読み取り処理の追加 +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/functional/equal_to.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/functional/equal_to.hpp index 4d58d05af8e..7657557f53d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/functional/equal_to.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/functional/equal_to.hpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -37,6 +38,26 @@ struct equal_to::value>::ty return std::abs(lhs - rhs) < std::numeric_limits::type>::epsilon(); } }; + +template +struct equal_to, typename std::enable_if::value>::type> +{ + constexpr auto operator()( + const std::valarray & lhs, const std::valarray & rhs) const noexcept + { + return std::abs(lhs - rhs) < std::numeric_limits::type>::epsilon(); + } + + constexpr auto operator()(const std::valarray & lhs, const T & rhs) const noexcept + { + return std::abs(lhs - rhs) < std::numeric_limits::type>::epsilon(); + } + + constexpr auto operator()(const T & lhs, const std::valarray & rhs) const noexcept + { + return std::abs(lhs - rhs) < std::numeric_limits::type>::epsilon(); + } +}; } // namespace syntax } // namespace openscenario_interpreter 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/scope.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp index 4f6cfe13ed5..ca02c31d8e4 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/scope.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -190,7 +190,7 @@ class Scope public: const std::string name; - std::list actors; + std::list actors; double seed; // NOTE: `seed` is used only for sharing randomSeed in Stochastic now diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 2e7e89f6558..fc3a3f9edb6 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 @@ -334,8 +356,47 @@ class SimulatorCore }()); if (controller.isAutoware()) { - 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("tamagawa/imu_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/acceleration_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/acceleration_condition.hpp index cadfd0633db..2ae30f85401 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/acceleration_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/acceleration_condition.hpp @@ -21,6 +21,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -42,7 +43,7 @@ struct AccelerationCondition : private SimulatorCore::ConditionEvaluation const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector> results; // for description explicit AccelerationCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/by_type.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/by_type.hpp index d1fbb0e15dc..9f05184451b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/by_type.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/by_type.hpp @@ -15,7 +15,9 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__BY_TYPE_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__BY_TYPE_HPP_ +#include #include +#include namespace openscenario_interpreter { @@ -28,8 +30,9 @@ inline namespace syntax * * * -------------------------------------------------------------------------- */ -struct ByType +struct ByType : public ObjectType { + explicit ByType(const pugi::xml_node &, Scope &); }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 61ea2db6345..d7f297bf54f 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -26,6 +26,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -105,7 +106,7 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector> results; // for description const bool consider_z; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp index a8abbd1dc91..d0c3d753aa4 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entities.hpp @@ -16,6 +16,7 @@ #define OPENSCENARIO_INTERPRETER__SYNTAX__ENTITIES_HPP_ #include +#include #include #include @@ -37,7 +38,7 @@ struct Entities : public std::unordered_map // TODO to be { explicit Entities(const pugi::xml_node &, Scope &); - auto isAdded(const EntityRef &) const -> bool; + auto isAdded(const Entity &) const -> bool; auto ref(const EntityRef &) const -> Object; }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity.hpp new file mode 100644 index 00000000000..24e9b8aa00c --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity.hpp @@ -0,0 +1,101 @@ +// 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__ENTITY_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +struct Scope; + +inline namespace syntax +{ +struct Entities; + +struct ScenarioObject; + +struct EntitySelection; + +struct Entity : public Object +{ + Entity() = default; + + Entity(const ScenarioObject &); + + Entity(const EntitySelection &); + + Entity(const String &, const Scope &); + + Entity(const pugi::xml_node &, const Scope &); + + auto name() const -> String; + + auto objects() const -> std::set; + + auto objectTypes() const -> std::set; + + template + auto apply(const Function & function) const + { + using Result = std::invoke_result_t; + auto objects = this->objects(); + if constexpr (std::is_same_v) { + std::for_each(std::begin(objects), std::end(objects), function); + } else { + auto results = std::valarray(objects.size()); + std::transform(std::begin(objects), std::end(objects), std::begin(results), function); + return results; + } + } + + /** + * This function is for ScenarioObject only. + * @throws std::runtime_error if the entity is not a ScenarioObject. + * @note To iterate over all objects, use `apply` function instead. + * @note To get the name of the entity, use `name` function instead. + */ + operator EntityRef() const; +}; + +auto operator==(const Entity &, const Entity &) -> bool; + +auto operator<<(std::ostream &, const Entity &) -> std::ostream &; +} // namespace syntax +} // namespace openscenario_interpreter + +template <> +struct std::hash +{ + auto operator()(const openscenario_interpreter::Entity & entity) const -> std::size_t + { + return std::hash{}(entity.get()); + } +}; + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_action.hpp index 9849f6f5828..5b69762b93b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_action.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -38,7 +39,7 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct EntityAction : public ComplexType { - const String entity_ref; + const Entity entity_ref; explicit EntityAction(const pugi::xml_node &, Scope &); 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/entity_object.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_object.hpp index 3f0fc615d30..d7f3c630682 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_object.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_object.hpp @@ -16,6 +16,7 @@ #define OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_OBJECT_HPP_ #include +#include #include #include @@ -38,8 +39,18 @@ inline namespace syntax struct EntityObject : public Group { explicit EntityObject(const pugi::xml_node &, Scope &); + + auto objectType() const -> ObjectType::value_type; }; +DEFINE_LAZY_VISITOR( + const EntityObject, + // CASE(CatalogReference), // + CASE(Vehicle), // + CASE(Pedestrian), // + CASE(MiscObject), // +); + DEFINE_LAZY_VISITOR( EntityObject, // CASE(CatalogReference), // diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_ref.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_ref.hpp index bbaaf5a84bd..e0adf8a4298 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_ref.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_ref.hpp @@ -33,10 +33,7 @@ inline namespace syntax */ struct EntityRef : public String { - template - EntityRef(Ts &&... xs) : String(std::forward(xs)...) - { - } + EntityRef(const String & string) : String(string) {} template explicit EntityRef(const Node & node, Scope & scope) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_selection.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_selection.hpp index f8f4d97e378..acafe921276 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_selection.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/entity_selection.hpp @@ -15,7 +15,12 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_SELECTION_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__ENTITY_SELECTION_HPP_ +#include +#include +#include +#include #include +#include namespace openscenario_interpreter { @@ -31,8 +36,13 @@ inline namespace syntax * * * -------------------------------------------------------------------------- */ -struct EntitySelection +struct EntitySelection : public Scope, public SelectedEntities { + explicit EntitySelection(const pugi::xml_node &, Scope &); + + auto objects() const -> std::set; + + auto objectTypes() const -> std::set; }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/external_object_reference.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/external_object_reference.hpp new file mode 100644 index 00000000000..2476ba397cd --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/external_object_reference.hpp @@ -0,0 +1,45 @@ +// 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__EVENT_HPP_ +#define OPENSCENARIO_INTERPRETER__SYNTAX__EVENT_HPP_ + +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +/* ---- ExternalObjectReference ------------------------------------------------ + * + * + * + * + * + * -------------------------------------------------------------------------- */ +struct ExternalObjectReference +{ + static constexpr ObjectType object_type{ObjectType::external}; + + const String name; + + explicit ExternalObjectReference(const pugi::xml_node &, Scope &); +}; +} // namespace syntax +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__SYNTAX__EVENT_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp index 73d5a6b6678..d821cb079d5 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -66,7 +67,7 @@ struct FollowTrajectoryAction : private Scope, const TrajectoryRef trajectory_ref; - std::unordered_map accomplishments; + std::unordered_map accomplishments; explicit FollowTrajectoryAction(const pugi::xml_node &, Scope &); 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/lane_change_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/lane_change_action.hpp index ea333af154c..7772aa49bba 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/lane_change_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/lane_change_action.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -52,7 +53,7 @@ struct LaneChangeAction : private Scope, explicit LaneChangeAction(const pugi::xml_node &, Scope &); - std::unordered_map accomplishments; + std::unordered_map accomplishments; /* */ auto accomplished() -> bool; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/misc_object.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/misc_object.hpp index 0e5d721ccbf..cf06ba28cbc 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/misc_object.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/misc_object.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,8 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct MiscObject : public Scope { + static constexpr ObjectType object_type{ObjectType::miscellaneous}; + const Double mass; // Mass of the miscellaneous object. Unit: Kg; Range: [0..inf[. const MiscObjectCategory misc_object_category; // Categorization of the miscellaneous object. diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/object_type.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/object_type.hpp index 61d2715140b..ff65964ee50 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/object_type.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/object_type.hpp @@ -15,6 +15,9 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__OBJECT_TYPE_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__OBJECT_TYPE_HPP_ +#include +#include + namespace openscenario_interpreter { inline namespace syntax @@ -39,7 +42,22 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct ObjectType { + enum value_type { + vehicle, + + external, + miscellaneous, + pedestrian, + } value; + + ObjectType() = default; + + operator value_type() const noexcept { return value; } }; + +auto operator>>(std::istream &, ObjectType &) -> std::istream &; + +auto operator<<(std::ostream &, const ObjectType &) -> std::ostream &; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/pedestrian.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/pedestrian.hpp index 6368e2c7ea7..80efc9f19ad 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/pedestrian.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/pedestrian.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -45,6 +46,8 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct Pedestrian : public Scope { + static constexpr ObjectType object_type{ObjectType::pedestrian}; + const Double mass; const String model; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/private.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/private.hpp index c3910d8b93f..b265a4045be 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/private.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/private.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -36,7 +37,7 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct Private : public Scope { - const String entity_ref; + const Entity entity_ref; std::list private_actions; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp index 527614ad500..0f981287244 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -47,7 +48,7 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector> results; // for description const bool consider_z; 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_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 39926681b26..80265549339 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -18,12 +18,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include namespace openscenario_interpreter { @@ -57,7 +59,7 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi /* Reference entity. */ - const String entity_ref; + const Entity entity_ref; /* True: distance is measured between closest bounding box points. @@ -88,7 +90,7 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector> results; // for description const bool consider_z; 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/relative_object_position.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_object_position.hpp index f5f76316449..5dbb9a7b40c 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_object_position.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_object_position.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ struct RelativeObjectPosition : private SimulatorCore::CoordinateSystemConversio { const Orientation orientation; - const String entity_ref; + const Entity entity_ref; const Double dx, dy, dz; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_lane.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_lane.hpp index 6c5b678e1f0..9b1d1844d6d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_lane.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_lane.hpp @@ -16,6 +16,7 @@ #define OPENSCENARIO_INTERPRETER__SYNTAX__RELATIVE_TARGET_LANE_HPP_ #include +#include #include #include @@ -33,7 +34,7 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct RelativeTargetLane { - const String entity_ref; + const Entity entity_ref; const Integer value; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_speed.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_speed.hpp index 7a70f3e6d9f..fef65f54e72 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_speed.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_target_speed.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -39,7 +39,7 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct RelativeTargetSpeed { - const String entity_ref; + const Entity entity_ref; const Double value; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_world_position.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_world_position.hpp index 624ad004aac..a8a204b2159 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_world_position.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_world_position.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ struct RelativeWorldPosition : private SimulatorCore::CoordinateSystemConversion { const Orientation orientation; - const String entity_ref; + const Entity entity_ref; const Double dx, dy, dz; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/rule.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/rule.hpp index aa05246a98d..56b2e557bff 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/rule.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/rule.hpp @@ -19,11 +19,23 @@ #include #include #include +#include namespace openscenario_interpreter { inline namespace syntax { +template +struct RuleResultDeduction +{ + using type = bool; +}; + +template +struct RuleResultDeduction, U> +{ + using type = std::valarray; +}; /* Rule (OpenSCENARIO XML 1.3) @@ -63,28 +75,27 @@ struct Rule constexpr operator value_type() const noexcept { return value; } template - constexpr auto operator()(const T & lhs, const U & rhs) const noexcept + constexpr auto operator()(const T & lhs, const U & rhs) const noexcept -> + typename RuleResultDeduction::type { switch (value) { case equalTo: return equal_to()(std::forward(lhs), std::forward(rhs)); case greaterThan: - return std::greater()( - std::forward(lhs), std::forward(rhs)); + return std::greater()(std::forward(lhs), std::forward(rhs)); case lessThan: - return std::less()( - std::forward(lhs), std::forward(rhs)); + return std::less()(std::forward(lhs), std::forward(rhs)); case greaterOrEqual: - return std::greater_equal()( + return std::greater_equal()( std::forward(lhs), std::forward(rhs)); case lessOrEqual: - return std::less_equal()( + return std::less_equal()( std::forward(lhs), std::forward(rhs)); case notEqualTo: - return std::not_equal_to()( + return std::not_equal_to()( std::forward(lhs), std::forward(rhs)); default: - return false; + return {}; } } }; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/selected_entities.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/selected_entities.hpp index 49bba699166..a84a3bea949 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/selected_entities.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/selected_entities.hpp @@ -15,8 +15,10 @@ #ifndef OPENSCENARIO_INTERPRETER__SYNTAX__SELECTED_ENTITIES_HPP_ #define OPENSCENARIO_INTERPRETER__SYNTAX__SELECTED_ENTITIES_HPP_ +#include +#include #include -#include +#include namespace openscenario_interpreter { @@ -32,8 +34,13 @@ inline namespace syntax * * * -------------------------------------------------------------------------- */ -struct SelectedEntities +struct SelectedEntities : public ComplexType { + const std::list entityRef; + + const std::list byType; + + explicit SelectedEntities(const pugi::xml_node &, Scope &); }; } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp index 54f7eded7eb..c37b96b513f 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp @@ -47,7 +47,7 @@ struct SpeedAction : private Scope, // NOTE: Required for access to actors explicit SpeedAction(const pugi::xml_node &, Scope &); - std::unordered_map accomplishments; + std::unordered_map accomplishments; auto accomplished() -> bool; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp index 29d0be6eb99..c33ac44e1f7 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp @@ -21,6 +21,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -45,7 +46,7 @@ struct SpeedCondition : private SimulatorCore::ConditionEvaluation const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector> results; // for description explicit SpeedCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp index 2fea3279574..19550531412 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -47,7 +47,7 @@ struct SpeedProfileAction : private Scope, // NOTE: Required for access to acto Reference entity. If set, the speed values will be interpreted as relative delta to the speed of the referenced entity. */ - const EntityRef entity_ref; + const Entity entity_ref; /* Defines whether to apply strictly linear interpolation between speed @@ -68,13 +68,13 @@ struct SpeedProfileAction : private Scope, // NOTE: Required for access to acto const std::list speed_profile_entry; // Defines a series of speed targets. - std::unordered_map::const_iterator> accomplishments; + std::unordered_map::const_iterator> accomplishments; explicit SpeedProfileAction(const pugi::xml_node &, Scope &); auto accomplished() -> bool; - auto apply(const EntityRef &, const SpeedProfileEntry &) -> void; + auto apply(const Entity &, const SpeedProfileEntry &) -> void; auto endsImmediately() const -> bool; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stand_still_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stand_still_condition.hpp index bcdf91e29ae..f15fb1ff192 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stand_still_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/stand_still_condition.hpp @@ -21,6 +21,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -41,7 +42,7 @@ struct StandStillCondition : private SimulatorCore::ConditionEvaluation const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector> results; // for description explicit StandStillCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_headway_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_headway_condition.hpp index 40b1711fb06..6a35998fe35 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_headway_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/time_headway_condition.hpp @@ -19,10 +19,12 @@ #include #include #include +#include #include #include #include #include +#include namespace openscenario_interpreter { @@ -45,7 +47,7 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct TimeHeadwayCondition : private SimulatorCore::ConditionEvaluation { - const String entity_ref; + const Entity entity_ref; const Double value; @@ -57,7 +59,7 @@ struct TimeHeadwayCondition : private SimulatorCore::ConditionEvaluation const TriggeringEntities triggering_entities; - std::vector results; // for description + std::vector> results; // for description explicit TimeHeadwayCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/triggering_entities.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/triggering_entities.hpp index 9b68037188c..feb5c0c640e 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/triggering_entities.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/triggering_entities.hpp @@ -16,6 +16,7 @@ #define OPENSCENARIO_INTERPRETER__SYNTAX__TRIGGERING_ENTITIES_HPP_ #include +#include #include #include #include @@ -39,7 +40,7 @@ struct TriggeringEntities { const TriggeringEntitiesRule triggering_entities_rule; - const std::list entity_refs; + const std::list entity_refs; explicit TriggeringEntities(const pugi::xml_node &, Scope &); 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/include/openscenario_interpreter/syntax/vehicle.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/vehicle.hpp index 8f7005132b5..84fddcdcd78 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/vehicle.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/vehicle.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -47,6 +48,8 @@ inline namespace syntax * -------------------------------------------------------------------------- */ struct Vehicle : public Scope // for ParameterDeclarations { + static constexpr ObjectType object_type{ObjectType::vehicle}; + const VehicleCategory vehicle_category; // Category of the vehicle (bicycle, train,...). const String diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/type_traits/iterable.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/type_traits/iterable.hpp new file mode 100644 index 00000000000..649d49decfb --- /dev/null +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/type_traits/iterable.hpp @@ -0,0 +1,39 @@ +// 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__TYPE_TRAITS__HAS_ITERATOR_HPP_ +#define OPENSCENARIO_INTERPRETER__TYPE_TRAITS__HAS_ITERATOR_HPP_ + +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace type_traits +{ +template +struct Iterable : public std::false_type +{ +}; + +template +struct Iterable()), std::end(std::declval()))>> +: public std::true_type +{ +}; +} // namespace type_traits +} // namespace openscenario_interpreter + +#endif // OPENSCENARIO_INTERPRETER__TYPE_TRAITS__HAS_ITERATOR_HPP_ diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/utility/print.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/utility/print.hpp index f389a1c3b36..bf45e5a00c4 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/utility/print.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/utility/print.hpp @@ -17,21 +17,32 @@ #include #include +#include +#include +#include namespace openscenario_interpreter { inline namespace utility { -template -auto & print_to(std::ostream & os, const SequenceContainer & sequence_container) +template +auto print_to(std::ostream & os, const T & value) + -> std::enable_if_t::value, std::ostream &> { - const auto * separator = "["; + return os << value; +} - for (const auto & each : sequence_container) { - os << separator << each; - separator = ", "; +template +auto print_to(std::ostream & os, const T & iterable) -> std::enable_if_t< + not concepts::HasStreamOutputOperator::value and type_traits::Iterable::value, + std::ostream &> +{ + os << "["; + const auto * separator = ""; + for (const auto & value : iterable) { + os << std::exchange(separator, ", "); + print_to(os, value); } - return os << "]"; } diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index f6513a67575..04fb15e42d8 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 3.2.0 + 4.2.7 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index 9d55c737729..c5f5cf332c9 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -139,6 +139,7 @@ auto Interpreter::engage() const -> void { for (const auto & [name, scenario_object] : currentScenarioDefinition()->entities) { if ( + scenario_object.template is() and scenario_object.template as().is_added and scenario_object.template as().object_controller.isAutoware()) { asFieldOperatorApplication(name).engage(); @@ -152,7 +153,8 @@ auto Interpreter::engageable() const -> bool std::cbegin(currentScenarioDefinition()->entities), std::cend(currentScenarioDefinition()->entities), [this](const auto & each) { const auto & [name, scenario_object] = each; - return not scenario_object.template as().is_added or + return not scenario_object.template is() or + not scenario_object.template as().is_added or not scenario_object.template as().object_controller.isAutoware() or asFieldOperatorApplication(name).engageable(); }); @@ -164,7 +166,8 @@ auto Interpreter::engaged() const -> bool std::cbegin(currentScenarioDefinition()->entities), std::cend(currentScenarioDefinition()->entities), [this](const auto & each) { const auto & [name, scenario_object] = each; - return not scenario_object.template as().is_added or + return not scenario_object.template is() or + not scenario_object.template as().is_added or not scenario_object.template as().object_controller.isAutoware() or asFieldOperatorApplication(name).engaged(); }); diff --git a/openscenario/openscenario_interpreter/src/syntax/acceleration_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/acceleration_condition.cpp index 8b06810ca1c..cdafc6fef55 100644 --- a/openscenario/openscenario_interpreter/src/syntax/acceleration_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/acceleration_condition.cpp @@ -25,7 +25,7 @@ AccelerationCondition::AccelerationCondition( : value(readAttribute("value", node, scope)), compare(readAttribute("rule", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + results(triggering_entities.entity_refs.size(), {Double::nan()}) { } @@ -47,8 +47,9 @@ auto AccelerationCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(evaluateAcceleration(triggering_entity)); - return compare(results.back(), value); + results.push_back( + triggering_entity.apply([](const auto & object) { return evaluateAcceleration(object); })); + return not results.back().size() or compare(results.back(), value).min(); })); } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp b/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp index 89f2f6a4232..bede4240b96 100644 --- a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp @@ -43,7 +43,7 @@ auto AcquirePositionAction::start() -> void }); for (const auto & actor : actors) { - apply(acquire_position, position, actor); + actor.apply([&](const auto & object) { apply(acquire_position, position, object); }); } } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp b/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp index e5bd8f29c70..376e5f270dd 100644 --- a/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/assign_route_action.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include @@ -32,6 +33,15 @@ AssignRouteAction::AssignRouteAction(const pugi::xml_node & node, Scope & scope) std::make_pair("CatalogReference", [&](auto && node) { return CatalogReference(node, local()).make(); }))) // clang-format on { + // OpenSCENARIO 1.2 Table 11 + for (const auto & actor : actors) { + if (auto object_types = actor.objectTypes(); object_types != std::set{ObjectType::vehicle} and + object_types != std::set{ObjectType::pedestrian}) { + THROW_SEMANTIC_ERROR( + "Actors may be either of vehicle type or a pedestrian type;" + "See OpenSCENARIO 1.2 Table 11 for more details"); + } + } } auto AssignRouteAction::accomplished() noexcept -> bool { return true; } @@ -43,8 +53,10 @@ auto AssignRouteAction::run() -> void {} auto AssignRouteAction::start() -> void { for (const auto & actor : actors) { - applyAssignRouteAction( - actor, static_cast>(route.as())); + actor.apply([&](const auto & object) { + applyAssignRouteAction( + object, static_cast>(route.as())); + }); } } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/by_type.cpp b/openscenario/openscenario_interpreter/src/syntax/by_type.cpp new file mode 100644 index 00000000000..bfaea2ae800 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/by_type.cpp @@ -0,0 +1,27 @@ +// 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 +{ +ByType::ByType(const pugi::xml_node & tree, Scope & scope) +: ObjectType(readAttribute("objectType", tree, scope)) +{ +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/collision_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/collision_condition.cpp index 02d4e8ea5e7..24651b21773 100644 --- a/openscenario/openscenario_interpreter/src/syntax/collision_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/collision_condition.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include namespace openscenario_interpreter @@ -28,7 +29,7 @@ CollisionCondition::CollisionCondition( : Scope(scope), another_given_entity( choice(node, - std::make_pair("EntityRef", [&](auto && node) { return make(node, scope); }), + std::make_pair("EntityRef", [&](auto && node) { return make(node, scope); }), std::make_pair("ByType", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }))), triggering_entities(triggering_entities) // clang-format on @@ -51,10 +52,13 @@ auto CollisionCondition::description() const -> std::string auto CollisionCondition::evaluate() const -> Object { if ( - another_given_entity.is() and - global().entities->isAdded(another_given_entity.as())) { + another_given_entity.is() and + global().entities->isAdded(another_given_entity.as())) { return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - return evaluateCollisionCondition(triggering_entity, another_given_entity.as()); + auto evaluation = triggering_entity.apply([&](const auto & object) { + return evaluateCollisionCondition(object, another_given_entity.as()); + }); + return not evaluation.size() or evaluation.min(); })); } else { // TODO ByType diff --git a/openscenario/openscenario_interpreter/src/syntax/controller_action.cpp b/openscenario/openscenario_interpreter/src/syntax/controller_action.cpp index 5f8bafc72fa..f64d587c1f3 100644 --- a/openscenario/openscenario_interpreter/src/syntax/controller_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/controller_action.cpp @@ -14,6 +14,7 @@ #include #include +#include namespace openscenario_interpreter { @@ -26,6 +27,16 @@ ControllerAction::ControllerAction(const pugi::xml_node & node, Scope & scope) override_controller_value_action(readElement( "OverrideControllerValueAction", node, scope)) // NOTE: DUMMY IMPLEMENTATION { + // OpenSCENARIO 1.2 Table 11 + for (const auto & actor : actors) { + for (const auto & object_type : actor.objectTypes()) { + if (object_type != ObjectType::vehicle and object_type != ObjectType::pedestrian) { + THROW_SEMANTIC_ERROR( + "Actors may be either of vehicle type or a pedestrian type;" + "See OpenSCENARIO 1.2 Table 11 for more details"); + } + } + } } auto ControllerAction::accomplished() noexcept -> bool // @@ -43,7 +54,7 @@ auto ControllerAction::run() noexcept -> void {} auto ControllerAction::start() const -> void { for (const auto & actor : actors) { - assign_controller_action(actor); + actor.apply(assign_controller_action); } } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp index a54c9731c54..5cdc099fd56 100644 --- a/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/custom_command_action.cpp @@ -191,7 +191,7 @@ struct ApplyWalkStraightAction : public CustomCommand, private SimulatorCore::Ac } for (const auto & actor : scope.actors) { - applyWalkStraightAction(actor); + actor.apply([&](const auto & object) { applyWalkStraightAction(object); }); } }; }; diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 4e817bb7286..fd90d905e38 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -46,7 +46,7 @@ DistanceCondition::DistanceCondition( value(readAttribute("value", node, scope)), position(readElement("Position", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()), + results(triggering_entities.entity_refs.size(), {Double::nan()}), consider_z([]() { rclcpp::Node node{"get_parameter", "simulation"}; node.declare_parameter("consider_pose_by_road_slope", false); @@ -749,8 +749,9 @@ auto DistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(distance(triggering_entity)); - return rule(static_cast(results.back()), value); + results.push_back( + triggering_entity.apply([&](const auto & object) { return distance(object); })); + return not results.back().size() or rule(results.back(), value).min(); })); } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/entities.cpp b/openscenario/openscenario_interpreter/src/syntax/entities.cpp index 9c5872bacf9..abdcc3628c2 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entities.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entities.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include @@ -30,14 +31,15 @@ Entities::Entities(const pugi::xml_node & node, Scope & scope) scope.global().entities = this; traverse<0, unbounded>(node, "EntitySelection", [&](auto && node) { - throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); - return unspecified; + emplace(readAttribute("name", node, scope), make(node, scope)); }); } -auto Entities::isAdded(const EntityRef & entity_ref) const -> bool +auto Entities::isAdded(const Entity & entity_ref) const -> bool { - return ref(entity_ref).template as().is_added; + auto evaluation = entity_ref.apply( + [&](const auto & object) { return object.template as().is_added; }); + return not evaluation.size() or evaluation.min(); } auto Entities::ref(const EntityRef & entity_ref) const -> Object diff --git a/openscenario/openscenario_interpreter/src/syntax/entity.cpp b/openscenario/openscenario_interpreter/src/syntax/entity.cpp new file mode 100644 index 00000000000..ae036396219 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/entity.cpp @@ -0,0 +1,94 @@ +// 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 +#include +#include +#include +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +Entity::Entity(const ScenarioObject & object) : Object(make(object)) {} + +Entity::Entity(const EntitySelection & selection) : Object(make(selection)) {} + +Entity::Entity(const String & name, const Scope & scope) +: Object(name.empty() ? nullptr : scope.global().entities->ref(name)) +{ +} + +Entity::Entity(const pugi::xml_node & node, const Scope & scope) +: Entity(readAttribute("entityRef", node, scope), scope) +{ +} + +auto operator==(const Entity & left, const Entity & right) -> bool +{ + return left.get() == right.get(); +} + +auto operator<<(std::ostream & os, const Entity & entity) -> std::ostream & +{ + return os << (entity ? entity.name() : ""); +} + +auto Entity::name() const -> String { return this->as().name; } + +auto Entity::objects() const -> std::set +{ + if (is()) { + return {as()}; + } else if (is()) { + return as().objects(); + } else { + throw std::runtime_error{"Unexpected entity type is detected. This is a simulator bug."}; + } +} + +auto Entity::objectTypes() const -> std::set +{ + if (is()) { + return {as().objectType()}; + } else if (is()) { + return as().objectTypes(); + } else { + throw std::runtime_error{"Unexpected entity type is detected. This is a simulator bug."}; + } +} + +Entity::operator EntityRef() const +{ + if (is()) { + return name(); + } else { + throw std::runtime_error{ + "Entity::operator EntityRef() is called for non-ScenarioObject entity." + "This is a simulator bug."}; + } +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_action.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_action.cpp index 34bf20b92c1..cd04e3f50e3 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_action.cpp @@ -26,7 +26,7 @@ EntityAction::EntityAction(const pugi::xml_node & node, Scope & scope) choice(node, std::make_pair( "AddEntityAction", [&](auto && node) { return make< AddEntityAction>(node, scope); }), std::make_pair("DeleteEntityAction", [&](auto && node) { return make(node, scope); }))), - entity_ref(readAttribute("entityRef", node, scope)) + entity_ref(readAttribute("entityRef", node, scope), scope) // clang-format on { } @@ -39,7 +39,7 @@ auto EntityAction::run() noexcept -> void {} auto EntityAction::start() const -> void { - return apply([this](auto && action) { return action(entity_ref); }, *this); + apply([&](auto && action) { entity_ref.apply(action); }, *this); } } // namespace syntax } // namespace openscenario_interpreter 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/entity_object.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_object.cpp index 71b14d6caa2..e345d2237b9 100644 --- a/openscenario/openscenario_interpreter/src/syntax/entity_object.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/entity_object.cpp @@ -15,6 +15,7 @@ #include #include #include +#include namespace openscenario_interpreter { @@ -31,5 +32,11 @@ EntityObject::EntityObject(const pugi::xml_node & node, Scope & scope) // clang-format on { } + +auto EntityObject::objectType() const -> ObjectType::value_type +{ + return apply( + [](const auto & object) { return object.object_type; }, *this); +} } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/entity_selection.cpp b/openscenario/openscenario_interpreter/src/syntax/entity_selection.cpp new file mode 100644 index 00000000000..48f4c74a4a9 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/entity_selection.cpp @@ -0,0 +1,69 @@ +// 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 +#include +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +EntitySelection::EntitySelection(const pugi::xml_node & node, Scope & scope) +: Scope(readAttribute("name", node, scope), scope), + SelectedEntities(readElement("Members", node, scope)) +{ +} + +auto EntitySelection::objects() const -> std::set +{ + auto result = std::set{}; + if (not entityRef.empty()) { + for (const auto & entity_ref : entityRef) { + result.merge(entity_ref.objects()); + } + } else { + auto types = objectTypes(); + for (const auto & [name, object] : *global().entities) { + if (object.is() and types.count(object.as().objectType())) { + result.emplace(name, *this); + } + } + } + return result; +} + +auto EntitySelection::objectTypes() const -> std::set +{ + auto result = std::set{}; + if (not entityRef.empty()) { + for (const auto & entity_ref : entityRef) { + result.merge(entity_ref.objectTypes()); + } + } else { + result.insert(byType.cbegin(), byType.cend()); + } + return result; +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/external_object_reference.cpp b/openscenario/openscenario_interpreter/src/syntax/external_object_reference.cpp new file mode 100644 index 00000000000..ab29ff3124e --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/external_object_reference.cpp @@ -0,0 +1,27 @@ +// 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 +{ +ExternalObjectReference::ExternalObjectReference(const pugi::xml_node & node, Scope & scope) +: name(readAttribute("name", node, scope)) +{ +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/follow_trajectory_action.cpp b/openscenario/openscenario_interpreter/src/syntax/follow_trajectory_action.cpp index a4ac8d5fb06..23bee1c5243 100644 --- a/openscenario/openscenario_interpreter/src/syntax/follow_trajectory_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/follow_trajectory_action.cpp @@ -53,10 +53,12 @@ auto FollowTrajectoryAction::accomplished() -> bool } else { return std::all_of( std::begin(accomplishments), std::end(accomplishments), [this](auto && accomplishment) { - auto is_running = [this](auto &&... xs) { + auto is_running = [this](const auto & actor) { if (trajectory_ref.trajectory.as().shape.is()) { - return evaluateCurrentState(std::forward(xs)...) == - "follow_polyline_trajectory"; + auto evaluation = actor.apply([&](const auto & object) { + return evaluateCurrentState(object) == "follow_polyline_trajectory"; + }); + return not evaluation.size() or evaluation.min(); } else { return false; } @@ -112,7 +114,7 @@ auto FollowTrajectoryAction::start() -> void parameter->closed = trajectory_ref.trajectory.as().closed; parameter->shape = repack_trajectory(); - applyFollowTrajectoryAction(actor, parameter); + actor.apply([&](const auto & object) { applyFollowTrajectoryAction(object, parameter); }); } } } // namespace syntax 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/lane_change_action.cpp b/openscenario/openscenario_interpreter/src/syntax/lane_change_action.cpp index c9f62538e12..aa442934da1 100644 --- a/openscenario/openscenario_interpreter/src/syntax/lane_change_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/lane_change_action.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include namespace openscenario_interpreter @@ -30,13 +31,25 @@ LaneChangeAction::LaneChangeAction(const pugi::xml_node & node, Scope & scope) readElement("LaneChangeActionDynamics", node, local())), lane_change_target(readElement("LaneChangeTarget", node, local())) { + // OpenSCENARIO 1.2 Table 11 + for (const auto & actor : actors) { + for (const auto & object_type : actor.objectTypes()) { + if (object_type != ObjectType::vehicle and object_type != ObjectType::pedestrian) { + THROW_SEMANTIC_ERROR( + "Actors may be either of vehicle type or a pedestrian type;" + "See OpenSCENARIO 1.2 Table 11 for more details"); + } + } + } } auto LaneChangeAction::accomplished() -> bool { - return std::all_of(std::begin(accomplishments), std::end(accomplishments), [](auto && each) { - const auto is_lane_changing = [](auto &&... xs) { - return evaluateCurrentState(std::forward(xs)...) == "lane_change"; + return std::all_of(std::begin(accomplishments), std::end(accomplishments), [&](auto & each) { + const auto is_lane_changing = [&](const auto & actor) { + auto evaluation = actor.apply( + [&](const auto & object) { return evaluateCurrentState(object) == "lane_change"; }); + return not evaluation.size() or evaluation.min(); }; return each.second or (each.second = not is_lane_changing(each.first)); }); @@ -56,17 +69,21 @@ auto LaneChangeAction::start() -> void for (const auto & accomplishment : accomplishments) { if (lane_change_target.is()) { - return applyLaneChangeAction( - accomplishment.first, static_cast(*this), - static_cast( - lane_change_action_dynamics.dynamics_shape), - static_cast(lane_change_action_dynamics)); + accomplishment.first.apply([&](const auto & object) { + applyLaneChangeAction( + object, static_cast(*this), + static_cast( + lane_change_action_dynamics.dynamics_shape), + static_cast(lane_change_action_dynamics)); + }); } else { - return applyLaneChangeAction( - accomplishment.first, static_cast(*this), - static_cast( - lane_change_action_dynamics.dynamics_shape), - static_cast(lane_change_action_dynamics)); + accomplishment.first.apply([&](const auto & object) { + applyLaneChangeAction( + object, static_cast(*this), + static_cast( + lane_change_action_dynamics.dynamics_shape), + static_cast(lane_change_action_dynamics)); + }); } } } diff --git a/openscenario/openscenario_interpreter/src/syntax/object_type.cpp b/openscenario/openscenario_interpreter/src/syntax/object_type.cpp new file mode 100644 index 00000000000..55e3b73c7a3 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/object_type.cpp @@ -0,0 +1,71 @@ +// 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 + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +auto operator>>(std::istream & is, ObjectType & type) -> std::istream & +{ + std::string buffer; + + is >> buffer; + +#define BOILERPLATE(IDENTIFIER) \ + if (buffer == #IDENTIFIER) { \ + type.value = ObjectType::IDENTIFIER; \ + return is; \ + } \ + static_assert(true, "") + + BOILERPLATE(vehicle); + BOILERPLATE(miscellaneous); + BOILERPLATE(pedestrian); + BOILERPLATE(external); + +#undef BOILERPLATE + + throw UNEXPECTED_ENUMERATION_VALUE_SPECIFIED(ObjectType, buffer); +} + +auto operator<<(std::ostream & os, const ObjectType & type) -> std::ostream & +{ +#define BOILERPLATE(IDENTIFIER) \ + case ObjectType::IDENTIFIER: \ + return os << #IDENTIFIER; + + switch (type) { + BOILERPLATE(vehicle); + BOILERPLATE(miscellaneous); + BOILERPLATE(pedestrian); + BOILERPLATE(external); + + default: + throw UNEXPECTED_ENUMERATION_VALUE_ASSIGNED(ObjectType, type); + } + +#undef BOILERPLATE +} + +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/private.cpp b/openscenario/openscenario_interpreter/src/syntax/private.cpp index 071ad2a03fe..e07192c43d7 100644 --- a/openscenario/openscenario_interpreter/src/syntax/private.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/private.cpp @@ -22,7 +22,7 @@ namespace openscenario_interpreter inline namespace syntax { Private::Private(const pugi::xml_node & node, Scope & scope) -: Scope(scope), entity_ref(readAttribute("entityRef", node, local())) +: Scope(scope), entity_ref(readAttribute("entityRef", node, local()), scope) { actors.emplace_back(entity_ref); @@ -87,7 +87,7 @@ auto Private::startNonInstantaneousActions() -> void auto operator<<(nlohmann::json & json, const Private & datum) -> nlohmann::json & { - json["entityRef"] = datum.entity_ref; + json["entityRef"] = datum.entity_ref.name(); json["PrivateAction"] = nlohmann::json::array(); diff --git a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp index 07dc4e81d06..e546d94f63f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp @@ -31,7 +31,7 @@ ReachPositionCondition::ReachPositionCondition( position(readElement("Position", node, scope)), compare(Rule::lessThan), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()), + results(triggering_entities.entity_refs.size(), {Double::nan()}), consider_z([]() { rclcpp::Node node{"get_parameter", "simulation"}; node.declare_parameter("consider_pose_by_road_slope", false); @@ -87,8 +87,9 @@ auto ReachPositionCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { - results.push_back(apply(distance, position, triggering_entity)); - return compare(results.back(), tolerance); + results.push_back(triggering_entity.apply( + [&](const auto & object) { return apply(distance, position, object); })); + return not results.back().size() or compare(results.back(), tolerance).min(); })); } } // namespace syntax 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_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 90e9b7be341..6988fd54dcf 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -31,7 +31,7 @@ RelativeDistanceCondition::RelativeDistanceCondition( : Scope(scope), coordinate_system( readAttribute("coordinateSystem", node, scope, CoordinateSystem::entity)), - entity_ref(readAttribute("entityRef", node, scope)), + entity_ref(readAttribute("entityRef", node, scope), scope), freespace(readAttribute("freespace", node, scope)), relative_distance_type(readAttribute("relativeDistanceType", node, scope)), routing_algorithm( @@ -39,7 +39,7 @@ RelativeDistanceCondition::RelativeDistanceCondition( rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()), + results(triggering_entities.entity_refs.size(), {Double::nan()}), consider_z([]() { rclcpp::Node node{"get_parameter", "simulation"}; node.declare_parameter("consider_pose_by_road_slope", false); @@ -418,8 +418,9 @@ auto RelativeDistanceCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) { - results.push_back(distance(triggering_entity)); - return rule(static_cast(results.back()), value); + results.push_back( + triggering_entity.apply([&](const auto & object) { return distance(object); })); + return not results.back().size() or rule(results.back(), value).min(); })); } } // namespace syntax 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/relative_object_position.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_object_position.cpp index 5691b478d3b..c7c5f4dfbc6 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_object_position.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_object_position.cpp @@ -23,7 +23,7 @@ inline namespace syntax { RelativeObjectPosition::RelativeObjectPosition(const pugi::xml_node & node, Scope & scope) : orientation(readElement("Orientation", node, scope)), - entity_ref(readAttribute("entityRef", node, scope)), + entity_ref(readAttribute("entityRef", node, scope), scope), dx(readAttribute("dx", node, scope)), dy(readAttribute("dy", node, scope)), dz(readAttribute("dz", node, scope, Double())) diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_target_lane.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_target_lane.cpp index bcf813c0b1d..8037d54064f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_target_lane.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_target_lane.cpp @@ -20,7 +20,7 @@ namespace openscenario_interpreter inline namespace syntax { RelativeTargetLane::RelativeTargetLane(const pugi::xml_node & node, Scope & scope) -: entity_ref(readAttribute("entityRef", node, scope)), +: entity_ref(readAttribute("entityRef", node, scope), scope), value(readAttribute("value", node, scope)) { } diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_target_speed.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_target_speed.cpp index 32e38dbe31c..cbae68020b6 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_target_speed.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_target_speed.cpp @@ -22,7 +22,7 @@ namespace openscenario_interpreter inline namespace syntax { RelativeTargetSpeed::RelativeTargetSpeed(const pugi::xml_node & node, Scope & scope) -: entity_ref(readAttribute("entityRef", node, scope)), +: entity_ref(readAttribute("entityRef", node, scope), scope), value(readAttribute("value", node, scope)), speed_target_value_type(readAttribute( "speedTargetValueType", node, scope, SpeedTargetValueType())), diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_world_position.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_world_position.cpp index b77525344a1..bfcbcaf00ea 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_world_position.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_world_position.cpp @@ -22,7 +22,7 @@ inline namespace syntax { RelativeWorldPosition::RelativeWorldPosition(const pugi::xml_node & node, Scope & scope) : orientation(readElement("Orientation", node, scope)), - entity_ref(readAttribute("entityRef", node, scope)), + entity_ref(readAttribute("entityRef", node, scope), scope), dx(readAttribute("dx", node, scope)), dy(readAttribute("dy", node, scope)), dz(readAttribute("dz", node, scope, Double())) diff --git a/openscenario/openscenario_interpreter/src/syntax/selected_entities.cpp b/openscenario/openscenario_interpreter/src/syntax/selected_entities.cpp new file mode 100644 index 00000000000..5aa542b76a7 --- /dev/null +++ b/openscenario/openscenario_interpreter/src/syntax/selected_entities.cpp @@ -0,0 +1,44 @@ +// 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 +#include + +namespace openscenario_interpreter +{ +inline namespace syntax +{ +SelectedEntities::SelectedEntities(const pugi::xml_node & tree, Scope & scope) +: entityRef(readElements("EntityRef", tree, scope)), + byType(readElements("ByType", tree, scope)) +{ + // clang-format off + // This function call is added to check the correctness of the syntax. + // DO NOT REMOVE unless syntax check is conducted in another way. + choice(tree, + std::pair{"EntityRef", [](const auto &) { return unspecified; }}, + std::pair{"ByType", [](const auto &) { return unspecified; }}); + // clang-format on +} +} // namespace syntax +} // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp index e72523eb949..ff317f7a48f 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_action.cpp @@ -15,7 +15,9 @@ #include #include #include +#include #include +#include namespace openscenario_interpreter { @@ -26,6 +28,16 @@ SpeedAction::SpeedAction(const pugi::xml_node & node, Scope & scope) speed_action_dynamics(readElement("SpeedActionDynamics", node, local())), speed_action_target(readElement("SpeedActionTarget", node, local())) { + // OpenSCENARIO 1.2 Table 11 + for (const auto & actor : actors) { + for (const auto & object_type : actor.objectTypes()) { + if (object_type != ObjectType::vehicle and object_type != ObjectType::pedestrian) { + THROW_SEMANTIC_ERROR( + "Actors may be either of vehicle type or a pedestrian type;" + "See OpenSCENARIO 1.2 Table 11 for more details"); + } + } + } } auto SpeedAction::accomplished() -> bool @@ -43,21 +55,28 @@ auto SpeedAction::accomplished() -> bool }; auto check = [this](auto && actor) { + auto evaluation = actor.apply([](const auto & object) { return evaluateSpeed(object); }); if (speed_action_target.is()) { - return equal_to()( - speed_action_target.as().value, evaluateSpeed(actor)); + return not evaluation.size() or + equal_to>()( + speed_action_target.as().value, evaluation) + .min(); } else { switch (speed_action_target.as().speed_target_value_type) { case SpeedTargetValueType::delta: - return equal_to()( - evaluateSpeed(speed_action_target.as().entity_ref) + - speed_action_target.as().value, - evaluateSpeed(actor)); + return not evaluation.size() or + equal_to>()( + evaluateSpeed(speed_action_target.as().entity_ref) + + speed_action_target.as().value, + evaluation) + .min(); case SpeedTargetValueType::factor: - return equal_to()( - evaluateSpeed(speed_action_target.as().entity_ref) * - speed_action_target.as().value, - evaluateSpeed(actor)); + return not evaluation.size() or + equal_to>()( + evaluateSpeed(speed_action_target.as().entity_ref) * + speed_action_target.as().value, + evaluation) + .min(); default: return false; } @@ -95,20 +114,24 @@ auto SpeedAction::start() -> void for (auto && each : accomplishments) { if (speed_action_target.is()) { - applySpeedAction( - std::get<0>(each), speed_action_target.as().value, - static_cast( - speed_action_dynamics.dynamics_shape), - static_cast(speed_action_dynamics), true); + each.first.apply([&](const auto & object) { + applySpeedAction( + object, speed_action_target.as().value, + static_cast( + speed_action_dynamics.dynamics_shape), + static_cast(speed_action_dynamics), true); + }); } else { - applySpeedAction( - std::get<0>(each), - static_cast( - speed_action_target.as()), - static_cast( - speed_action_dynamics.dynamics_shape), - static_cast(speed_action_dynamics), - speed_action_target.as().continuous); + each.first.apply([&](const auto & object) { + applySpeedAction( + object, + static_cast( + speed_action_target.as()), + static_cast( + speed_action_dynamics.dynamics_shape), + static_cast(speed_action_dynamics), + speed_action_target.as().continuous); + }); } } } diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp index 95f4f9b2497..b8e3a6640b8 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp @@ -26,7 +26,7 @@ SpeedCondition::SpeedCondition( : value(readAttribute("value", node, scope)), compare(readAttribute("rule", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + results(triggering_entities.entity_refs.size(), {Double::nan()}) { } @@ -48,8 +48,9 @@ auto SpeedCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(evaluateSpeed(triggering_entity)); - return compare(results.back(), value); + results.push_back( + triggering_entity.apply([&](const auto & object) { return evaluateSpeed(object); })); + return not results.back().size() or compare(results.back(), value).min(); })); } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp index accb647abf7..2e1ef370611 100644 --- a/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp @@ -15,7 +15,10 @@ #include #include #include +#include +#include #include +#include namespace openscenario_interpreter { @@ -23,16 +26,30 @@ inline namespace syntax { SpeedProfileAction::SpeedProfileAction(const pugi::xml_node & node, Scope & scope) : Scope(scope), - entity_ref(readAttribute("entityRef", node, scope, "")), + // NOTE: OpenSCENARIO User Guide does not state whether `EntitySelection` is allowed + // for `SpeedProfileAction.entityRef`. It seems `EntitySelection` can be used only when + // it is the subject of any actions or conditions, so I left default allowance of entities + // and `EntitySelection` is not allowed here. + entity_ref(readAttribute("entityRef", node, scope, String()), scope), following_mode(readAttribute("followingMode", node, scope)), dynamic_constraints( readElement("DynamicConstraints", node, scope, DynamicConstraints())), speed_profile_entry(readElements("SpeedProfileEntry", node, scope)) { + // OpenSCENARIO 1.2 Table 11 + for (const auto & actor : actors) { + for (const auto & object_type : actor.objectTypes()) { + if (object_type != ObjectType::vehicle and object_type != ObjectType::pedestrian) { + THROW_SEMANTIC_ERROR( + "Actors may be either of vehicle type or a pedestrian type;" + "See OpenSCENARIO 1.2 Table 11 for more details"); + } + } + } } -auto SpeedProfileAction::apply( - const EntityRef & actor, const SpeedProfileEntry & speed_profile_entry) -> void +auto SpeedProfileAction::apply(const Entity & actor, const SpeedProfileEntry & speed_profile_entry) + -> void { auto absolute_target_speed = [&]() { return speed_profile_entry.speed; }; @@ -71,16 +88,20 @@ auto SpeedProfileAction::apply( } }; - applyProfileAction(actor, dynamic_constraints); + actor.apply([&](const auto & object) { applyProfileAction(object, dynamic_constraints); }); - if (entity_ref.empty()) { - applySpeedAction( - actor, absolute_target_speed(), transition(), constraint(), - std::isnan(speed_profile_entry.time)); + if (entity_ref) { + actor.apply([&](const auto & object) { + applySpeedAction( + object, relative_target_speed(), transition(), constraint(), + std::isnan(speed_profile_entry.time)); + }); } else { - applySpeedAction( - actor, relative_target_speed(), transition(), constraint(), - std::isnan(speed_profile_entry.time)); + actor.apply([&](const auto & object) { + applySpeedAction( + object, absolute_target_speed(), transition(), constraint(), + std::isnan(speed_profile_entry.time)); + }); } } @@ -99,11 +120,15 @@ auto SpeedProfileAction::run() -> void { for (auto && [actor, iter] : accomplishments) { auto accomplished = [this](const auto & actor, const auto & speed_profile_entry) { - if (entity_ref.empty()) { - return equal_to()(evaluateSpeed(actor), speed_profile_entry.speed); + auto speeds = actor.apply([&](const auto & object) { return evaluateSpeed(object); }); + if (not speeds.size()) { + return true; + } else if (entity_ref) { + return equal_to>()( + speeds, speed_profile_entry.speed + evaluateSpeed(entity_ref)) + .min(); } else { - return equal_to()( - evaluateSpeed(actor), speed_profile_entry.speed + evaluateSpeed(entity_ref)); + return equal_to>()(speeds, speed_profile_entry.speed).min(); } }; diff --git a/openscenario/openscenario_interpreter/src/syntax/stand_still_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/stand_still_condition.cpp index e62f4d60ec2..9e95450f44a 100644 --- a/openscenario/openscenario_interpreter/src/syntax/stand_still_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/stand_still_condition.cpp @@ -25,7 +25,7 @@ StandStillCondition::StandStillCondition( : duration(readAttribute("duration", node, scope)), compare(Rule::greaterThan), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + results(triggering_entities.entity_refs.size(), {Double::nan()}) { } @@ -47,8 +47,9 @@ auto StandStillCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(evaluateStandStill(triggering_entity)); - return compare(results.back(), duration); + results.push_back( + triggering_entity.apply([&](const auto & object) { return evaluateStandStill(object); })); + return not results.back().size() or compare(results.back(), duration).min(); })); } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp index 4e59e603519..f53b6528687 100644 --- a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp @@ -16,6 +16,7 @@ #include #include #include // TEMPORARY (TODO REMOVE THIS LINE) +#include #include #include #include @@ -27,6 +28,13 @@ inline namespace syntax TeleportAction::TeleportAction(const pugi::xml_node & node, Scope & scope) : Scope(scope), position(readElement("Position", node, local())) { + // OpenSCENARIO 1.2 Table 11 + for (const auto & actor : actors) { + if (auto object_types = actor.objectTypes(); object_types.count(ObjectType::external)) { + THROW_SEMANTIC_ERROR( + "Actors cannot be ExternalObjectReference; See OpenSCENARIO 1.2 Table 11 for more details"); + } + } } auto TeleportAction::accomplished() noexcept -> bool { return true; } @@ -44,10 +52,10 @@ auto TeleportAction::run() noexcept -> void {} auto TeleportAction::start() const -> void { for (const auto & actor : actors) { - if (not global().entities->at(actor).as().is_added) { - AddEntityAction(local(), position)(actor); // NOTE: TIER IV extension + if (not global().entities->isAdded(actor)) { + actor.apply(AddEntityAction(local(), position)); // NOTE: TIER IV extension } else { - return teleport(actor, position); + actor.apply([&](const auto & object) { teleport(object, position); }); } } } diff --git a/openscenario/openscenario_interpreter/src/syntax/time_headway_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/time_headway_condition.cpp index c07d443bf18..f9482380138 100644 --- a/openscenario/openscenario_interpreter/src/syntax/time_headway_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/time_headway_condition.cpp @@ -22,13 +22,13 @@ inline namespace syntax { TimeHeadwayCondition::TimeHeadwayCondition( const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities) -: entity_ref(readAttribute("entityRef", node, scope)), +: entity_ref(readAttribute("entityRef", node, scope), scope), value(readAttribute("value", node, scope)), freespace(readAttribute("freespace", node, scope)), along_route(readAttribute("alongRoute", node, scope)), compare(readAttribute("rule", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + results(triggering_entities.entity_refs.size(), {Double::nan()}) { } @@ -51,8 +51,9 @@ auto TimeHeadwayCondition::evaluate() -> Object results.clear(); return asBoolean(triggering_entities.apply([&](auto && triggering_entity) { - results.push_back(evaluateTimeHeadway(triggering_entity, entity_ref)); - return compare(results.back(), value); + results.push_back(triggering_entity.apply( + [&](const auto & object) { return evaluateTimeHeadway(entity_ref, object); })); + return not results.back().size() or compare(results.back(), value).min(); })); } } // namespace syntax diff --git a/openscenario/openscenario_interpreter/src/syntax/triggering_entities.cpp b/openscenario/openscenario_interpreter/src/syntax/triggering_entities.cpp index 7cc7313f0b2..8597771444e 100644 --- a/openscenario/openscenario_interpreter/src/syntax/triggering_entities.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/triggering_entities.cpp @@ -24,7 +24,7 @@ inline namespace syntax TriggeringEntities::TriggeringEntities(const pugi::xml_node & node, Scope & scope) : triggering_entities_rule( readAttribute("triggeringEntitiesRule", node, scope)), - entity_refs(readElements("EntityRef", node, scope)) + entity_refs(readElements("EntityRef", node, scope)) { } 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 fd99947d906..29d0424434d 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp @@ -33,47 +33,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)), @@ -134,31 +164,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 0cc11f3526c..b66c7420bb9 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,232 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 43037e6986f..c49eee5d4ce 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 3.2.0 + 4.2.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 59f65e13fba..d336fd244ff 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,232 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 061d1e57011..3f306dff4dc 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 3.2.0 + 4.2.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 a6185fd509f..88f23930a07 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,234 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index ed712ad5eca..1451e68bfd2 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 3.2.0 + 4.2.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 bf55e88d96c..ebacc4b52b3 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,233 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index c3d8d67a870..be1055ed6a9 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 3.2.0 + 4.2.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 853ee08af75..b2dab44f9e2 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,232 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 75ce4803307..6376bba6dbe 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 3.2.0 + 4.2.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 d82a5a2597c..21b6c9c20b4 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,224 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 11508fdf77d..133896d8176 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 3.2.0 + 4.2.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 dd099e10bf9..11a9a156088 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,226 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index dc9c1217fcd..1a67d93e687 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 3.2.0 + 4.2.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 f9cf7d863ca..bf2fde2bbf4 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,229 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose 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 440c9623e9f..fa688c26a07 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.2.0 + 4.2.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 204dba3d277..05a97a5df18 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,235 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 66a1306d2aa..6f4ccc31483 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 3.2.0 + 4.2.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 704c2b58e1f..4805eb53181 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,251 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose 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 dea788adccd..67d0059618c 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 @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -65,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; @@ -84,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_light_manager"), BT::InputPort("request"), BT::OutputPort>("obstacle"), - BT::OutputPort>("updated_status"), BT::OutputPort("waypoints"), BT::OutputPort("request"), // clang-format on @@ -96,29 +94,30 @@ class ActionNode : public BT::ActionNodeBase } auto getBlackBoardValues() -> void; auto getEntityStatus(const std::string & target_name) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> const traffic_simulator::CanonicalizedEntityStatus &; auto getDistanceToTargetEntityPolygon( const math::geometry::CatmullRomSplineInterface & spline, const std::string target_name, 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::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; auto calculateUpdatedEntityStatusInWorldFrame( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; protected: traffic_simulator::behavior::Request request; std::shared_ptr hdmap_utils; std::shared_ptr traffic_light_manager; - 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 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 c241e49b9e0..edc05eb5d44 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 @@ -39,7 +39,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase { public: void configure(const rclcpp::Logger & logger) override; - void update(double current_time, double step_time) override; + auto update(const double current_time, const double step_time) -> void override; const std::string & getCurrentAction() const override; #define DEFINE_GETTER_SETTER(NAME, TYPE) \ @@ -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(TrafficLightManager, 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 @@ -76,7 +75,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase #undef DEFINE_GETTER_SETTER private: - BT::NodeStatus tickOnce(double current_time, double step_time); + auto tickOnce(const double current_time, const double step_time) -> BT::NodeStatus; auto createBehaviorTree(const std::string & format_path) -> BT::Tree; BT::BehaviorTreeFactory factory_; BT::Tree tree_; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp index 89b74eca32d..7d173c0b124 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/pedestrian_action_node.hpp @@ -45,9 +45,8 @@ class PedestrianActionNode : public ActionNode } traffic_simulator_msgs::msg::PedestrianParameters pedestrian_parameters; auto calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; - auto calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; + auto calculateUpdatedEntityStatus(double target_speed) const -> traffic_simulator::EntityStatus; protected: traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; 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 6add89d7ed4..59351fe827c 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 @@ -38,7 +38,7 @@ namespace entity_behavior class VehicleBehaviorTree : public BehaviorPluginBase { public: - void update(double current_time, double step_time) override; + auto update(const double current_time, const double step_time) -> void override; void configure(const rclcpp::Logger & logger) override; const std::string & getCurrentAction() const override; @@ -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,14 +72,13 @@ class VehicleBehaviorTree : public BehaviorPluginBase DEFINE_GETTER_SETTER(StepTime, double) DEFINE_GETTER_SETTER(TargetSpeed, std::optional) DEFINE_GETTER_SETTER(TrafficLightManager, 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 #undef DEFINE_GETTER_SETTER private: - BT::NodeStatus tickOnce(double current_time, double step_time); + auto tickOnce(const double current_time, const double step_time) -> BT::NodeStatus; auto createBehaviorTree(const std::string & format_path) -> BT::Tree; BT::BehaviorTreeFactory factory_; BT::Tree tree_; diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp index 1428e9f3199..024ede1096c 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/vehicle_action_node.hpp @@ -53,10 +53,9 @@ class VehicleActionNode : public ActionNode } return ports; } - auto calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; + auto calculateUpdatedEntityStatus(double target_speed) const -> traffic_simulator::EntityStatus; auto calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus; + -> traffic_simulator::EntityStatus; virtual const traffic_simulator_msgs::msg::WaypointsArray calculateWaypoints() = 0; virtual const std::optional calculateObstacle( const traffic_simulator_msgs::msg::WaypointsArray & waypoints) = 0; diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 4dd4fdaacc7..9565295b9b1 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 3.2.0 + 4.2.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 5f9b4501dac..29fcb393d7d 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_light_manager 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->laneMatchingSucceed()) { - const auto lanelet_pose = entity_status->getLaneletPose(); + if (!other_status.empty() && canonicalized_entity_status->laneMatchingSucceed()) { + 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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; } @@ -240,7 +248,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. @@ -277,12 +285,13 @@ auto ActionNode::getDistanceToTargetEntityOnCrosswalk( } auto ActionNode::getEntityStatus(const std::string & target_name) const - -> traffic_simulator::CanonicalizedEntityStatus + -> const traffic_simulator::CanonicalizedEntityStatus & { - if (other_entity_status.find(target_name) != other_entity_status.end()) { - return traffic_simulator::CanonicalizedEntityStatus(other_entity_status.at(target_name)); + if (auto it = other_entity_status.find(target_name); it != other_entity_status.end()) { + return it->second; + } else { + THROW_SEMANTIC_ERROR("other entity : ", target_name, " does not exist."); } - THROW_SIMULATION_ERROR("other entity : ", target_name, " does not exist."); } auto ActionNode::getDistanceToTargetEntityPolygon( @@ -290,7 +299,7 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_right, double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - const auto status = getEntityStatus(target_name); + const auto & status = getEntityStatus(target_name); if (status.laneMatchingSucceed()) { return getDistanceToTargetEntityPolygon( spline, status, width_extension_right, width_extension_left, length_extension_front, @@ -397,40 +406,45 @@ auto ActionNode::foundConflictingEntity(const lanelet::Ids & following_lanelets) auto ActionNode::calculateUpdatedEntityStatus( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { 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->laneMatchingSucceed()) { - THROW_SIMULATION_ERROR("Entity ", entity_status->getName(), " is not matched to the lanelet."); + if (!canonicalized_entity_status->laneMatchingSucceed()) { + 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. - traffic_simulator::EntityStatus entity_status_updated; + 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(); + entity_status_updated.lanelet_pose_valid = true; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; entity_status_updated.pose = hdmap_utils->toMapPose(canonicalized_lanelet_pose.value()).pose; } - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } else { // If canonicalize failed, set end of road lanelet pose. if (const auto end_of_road_lanelet_id = std::get>(canonicalized)) { @@ -442,16 +456,18 @@ auto ActionNode::calculateUpdatedEntityStatus( end_of_road_lanelet_pose.offset = lanelet_pose.offset; end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; } - traffic_simulator::EntityStatus entity_status_updated; + 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; + entity_status_updated.lanelet_pose_valid = true; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; } - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } else { traffic_simulator::LaneletPose end_of_road_lanelet_pose; { @@ -461,16 +477,18 @@ auto ActionNode::calculateUpdatedEntityStatus( end_of_road_lanelet_pose.offset = lanelet_pose.offset; end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; } - traffic_simulator::EntityStatus entity_status_updated; + 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; + entity_status_updated.lanelet_pose_valid = true; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; } - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } } else { THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id."); @@ -481,14 +499,15 @@ auto ActionNode::calculateUpdatedEntityStatus( auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { 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); @@ -497,44 +516,35 @@ 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; - traffic_simulator::EntityStatus entity_status_updated; + 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; entity_status_updated.pose = pose_new; entity_status_updated.action_status.twist = twist_new; entity_status_updated.action_status.accel = accel_new; entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.lanelet_pose_valid = false; - entity_status_updated.lanelet_pose = traffic_simulator::LaneletPose(); - return traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils); + return entity_status_updated; } 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/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp index bc083253ca0..4cfaf3a8ea5 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/behavior_tree.cpp @@ -83,7 +83,7 @@ const std::string & PedestrianBehaviorTree::getCurrentAction() const return logging_event_ptr_->getCurrentAction(); } -void PedestrianBehaviorTree::update(double current_time, double step_time) +auto PedestrianBehaviorTree::update(const double current_time, const double step_time) -> void { tickOnce(current_time, step_time); while (getCurrentAction() == "root") { @@ -91,7 +91,8 @@ void PedestrianBehaviorTree::update(double current_time, double step_time) } } -BT::NodeStatus PedestrianBehaviorTree::tickOnce(double current_time, double step_time) +auto PedestrianBehaviorTree::tickOnce(const double current_time, const double step_time) + -> BT::NodeStatus { setCurrentTime(current_time); setStepTime(step_time); 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 06833c75b3a..c24d13d66b1 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_lane_action.cpp @@ -38,18 +38,16 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { stopEntity(); - setOutput("updated_status", 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( - "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 7e3ca5a6592..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,31 +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( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(*updated_status, hdmap_utils))); + 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 1f6f0630ae0..cbf2f304c22 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -42,31 +42,30 @@ void PedestrianActionNode::getBlackBoardValues() } auto PedestrianActionNode::calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { return ActionNode::calculateUpdatedEntityStatus( target_speed, behavior_parameter.dynamic_constraints); } auto PedestrianActionNode::calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { - auto updated_status = static_cast( - ActionNode::calculateUpdatedEntityStatusInWorldFrame( - target_speed, behavior_parameter.dynamic_constraints)); - + 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 traffic_simulator::CanonicalizedEntityStatus(updated_status, hdmap_utils); + 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 a58f7a8a30c..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,9 +47,7 @@ BT::NodeStatus WalkStraightAction::tick() if (!target_speed) { target_speed = 1.111; } - setOutput( - "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/behavior_tree.cpp b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp index 4a9e22c1c56..fbb49c65d63 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/behavior_tree.cpp @@ -142,7 +142,7 @@ const std::string & VehicleBehaviorTree::getCurrentAction() const return logging_event_ptr_->getCurrentAction(); } -void VehicleBehaviorTree::update(double current_time, double step_time) +auto VehicleBehaviorTree::update(const double current_time, const double step_time) -> void { tickOnce(current_time, step_time); while (getCurrentAction() == "root") { @@ -150,7 +150,8 @@ void VehicleBehaviorTree::update(double current_time, double step_time) } } -BT::NodeStatus VehicleBehaviorTree::tickOnce(double current_time, double step_time) +auto VehicleBehaviorTree::tickOnce(const double current_time, const double step_time) + -> BT::NodeStatus { setCurrentTime(current_time); setStepTime(step_time); 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 5535262b4bf..8c30fce024a 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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( @@ -111,15 +111,13 @@ BT::NodeStatus FollowFrontEntityAction::tick() return BT::NodeStatus::FAILURE; } } - auto front_entity_status = getEntityStatus(front_entity_name.value()); + const auto & front_entity_status = getEntityStatus(front_entity_name.value()); if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } const double front_entity_linear_velocity = front_entity_status.getTwist().linear.x; if (target_speed.value() <= front_entity_linear_velocity) { - setOutput( - "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,25 +127,19 @@ BT::NodeStatus FollowFrontEntityAction::tick() distance_to_front_entity_.value() >= (calculateStopDistance(behavior_parameter.dynamic_constraints) + vehicle_parameters.bounding_box.dimensions.x + 5)) { - setOutput( - "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( - "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( - "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 036c5409a04..1b70dd26599 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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,9 +74,8 @@ BT::NodeStatus FollowLaneAction::tick() request != traffic_simulator::behavior::Request::FOLLOW_LANE) { return BT::NodeStatus::FAILURE; } - if (!entity_status->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { stopEntity(); - setOutput("updated_status", entity_status); return BT::NodeStatus::RUNNING; } const auto waypoints = calculateWaypoints(); @@ -129,9 +128,7 @@ BT::NodeStatus FollowLaneAction::tick() if (!target_speed) { target_speed = hdmap_utils->getSpeedLimit(route_lanelets); } - setOutput( - "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 051452aee52..d1412b22029 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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( - "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 2ca745bd199..c85e6cdf622 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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( - "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( - "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 8e31993f431..23011a89e47 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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( - "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( - "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( - "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 823ae227cd3..e8033ff1ad7 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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( - "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( - "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 74e44e45393..002db268065 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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( - "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( - "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 98466d1c735..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,35 +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( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(*updated_status, hdmap_utils))); + 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 f5b381ab989..593915b94b0 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->laneMatchingSucceed()) { + if (!canonicalized_entity_status->laneMatchingSucceed()) { 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; @@ -115,7 +115,7 @@ BT::NodeStatus LaneChangeAction::tick() switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: /** - @note Hard coded parameter, + @note Hard coded parameter, 10.0 is a maximum_curvature_threshold (If the curvature of the trajectory is over 10.0, the trajectory was not selected.) 20.0 is a target_trajectory_length (The one with the closest length to 20 m is selected from the candidate trajectories.) 1.0 is a forward_distance_threshold (If the goal x position in the cartesian coordinate was under 1.0, the goal was rejected.) @@ -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,33 +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); - traffic_simulator::EntityStatus entity_status_updated; + auto entity_status_updated = + static_cast(*canonicalized_entity_status); entity_status_updated.pose = pose; - auto lanelet_pose = hdmap_utils->toLaneletPose(pose, entity_status->getBoundingBox(), false); - if (lanelet_pose) { - entity_status_updated.lanelet_pose = lanelet_pose.value(); - } else { - entity_status_updated.lanelet_pose_valid = false; - } - entity_status_updated.action_status = getActionStatus(); - setOutput( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils))); + entity_status_updated.lanelet_pose_valid = false; + 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; @@ -252,18 +246,17 @@ BT::NodeStatus LaneChangeAction::tick() curve_ = std::nullopt; current_s_ = 0; lane_change_velocity_ = 0; - traffic_simulator::EntityStatus entity_status_updated; + 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; lanelet_pose.offset = 0; - entity_status_updated.pose = hdmap_utils->toMapPose(lanelet_pose).pose; entity_status_updated.lanelet_pose = lanelet_pose; - entity_status_updated.action_status = getActionStatus(); - setOutput( - "updated_status", - std::make_shared( - traffic_simulator::CanonicalizedEntityStatus(entity_status_updated, hdmap_utils))); + entity_status_updated.lanelet_pose_valid = true; + entity_status_updated.pose = hdmap_utils->toMapPose(lanelet_pose).pose; + 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 6fef4b17172..549ba448d97 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/vehicle_action_node.cpp @@ -44,19 +44,19 @@ void VehicleActionNode::getBlackBoardValues() } auto VehicleActionNode::calculateUpdatedEntityStatus(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { return ActionNode::calculateUpdatedEntityStatus( target_speed, behavior_parameter.dynamic_constraints); } auto VehicleActionNode::calculateUpdatedEntityStatusInWorldFrame(double target_speed) const - -> traffic_simulator::CanonicalizedEntityStatus + -> traffic_simulator::EntityStatus { 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 367027180f7..683aaece85f 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,248 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose 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 7f82deee99a..482e3411ff7 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 747aa29d816..0c6523af9bd 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 3.2.0 + 4.2.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 ca84dc72406..8cc19f6a727 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 @@ -98,23 +100,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)); @@ -149,24 +164,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( - traffic_simulator::CanonicalizedEntityStatus(interpolated_status.value(), getHdMapUtils())); + getPolylineTrajectory(), getCanonicalizedEntityStatus(), getCurrentTime(), + getStepTime())) { + return interpolated_status.value(); } else { - return 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(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 0ebaeb1a77c..06acabf3c45 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,273 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index 4e3d828ec97..5c3eede4d60 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/occupancy_grid_sensor.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 6927a364d4f..20f274f3276 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 @@ -114,12 +115,22 @@ class SensorSimulation } } + 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/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/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index c10a521ba84..782d495a08c 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -92,10 +92,11 @@ class EgoEntitySimulation const bool consider_acceleration_by_road_slope); auto overwrite( - const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time, - double step_time, bool npc_logic_started) -> void; + const traffic_simulator_msgs::msg::EntityStatus & status, const double current_time, + const double step_time, bool is_npc_logic_started) -> void; - auto update(double time, double step_time, bool npc_logic_started) -> void; + auto update(const double current_time, const double step_time, const bool is_npc_logic_started) + -> void; auto requestSpeedChange(double value) -> void; @@ -103,7 +104,7 @@ class EgoEntitySimulation auto setStatus(const traffic_simulator_msgs::msg::EntityStatus & status) -> void; - auto updateStatus(double time, double step_time) -> void; + auto updateStatus(const double current_time, const double step_time) -> void; }; } // namespace vehicle_simulation diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 47557cb085c..bf5f7bc4636 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 3.2.0 + 4.2.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 4f3c1130989..b762ac6453b 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 75080db5ad3..23ac33eeb3f 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 @@ -218,9 +218,9 @@ void EgoEntitySimulation::requestSpeedChange(double value) vehicle_model_ptr_->setState(v); } -void EgoEntitySimulation::overwrite( - const traffic_simulator_msgs::msg::EntityStatus & status, double current_scenario_time, - double step_time, bool npc_logic_started) +auto EgoEntitySimulation::overwrite( + const traffic_simulator_msgs::msg::EntityStatus & status, const double current_time, + const double step_time, const bool is_npc_logic_started) -> void { using math::geometry::convertQuaternionToEulerAngle; using math::geometry::getRotationMatrix; @@ -238,7 +238,7 @@ void EgoEntitySimulation::overwrite( status.pose.position.y - initial_pose_.position.y, status.pose.position.z - initial_pose_.position.z); - if (npc_logic_started) { + if (is_npc_logic_started) { const auto yaw = [&]() { const auto q = Eigen::Quaterniond( getRotationMatrix(initial_pose_.orientation).transpose() * @@ -280,12 +280,12 @@ void EgoEntitySimulation::overwrite( "Unsupported simulation model ", toString(vehicle_model_type_), " specified"); } } - updateStatus(current_scenario_time, step_time); + updateStatus(current_time, step_time); updatePreviousValues(); } void EgoEntitySimulation::update( - double current_scenario_time, double step_time, bool npc_logic_started) + const double current_time, const double step_time, const bool is_npc_logic_started) { using math::geometry::getRotationMatrix; @@ -302,7 +302,7 @@ void EgoEntitySimulation::update( status_.getMapPose().position.y - initial_pose_.position.y, status_.getMapPose().position.z - initial_pose_.position.z); - if (npc_logic_started) { + if (is_npc_logic_started) { auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU()); auto acceleration_by_slope = [this]() { @@ -345,7 +345,7 @@ void EgoEntitySimulation::update( // only the position in the Oz axis is left unchanged, the rest is taken from SimModelInterface world_relative_position_.x() = vehicle_model_ptr_->getX(); world_relative_position_.y() = vehicle_model_ptr_->getY(); - updateStatus(current_scenario_time, step_time); + updateStatus(current_time, step_time); updatePreviousValues(); } @@ -464,6 +464,8 @@ auto EgoEntitySimulation::getStatus() const -> const traffic_simulator_msgs::msg auto EgoEntitySimulation::setStatus(const traffic_simulator_msgs::msg::EntityStatus & status) -> void { + /// @note The lanelet matching algorithm should be equivalent to the one used in + /// EgoEntity::setStatus const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets()); const auto matching_distance = std::max( @@ -471,17 +473,20 @@ auto EgoEntitySimulation::setStatus(const traffic_simulator_msgs::msg::EntitySta vehicle_parameters.axles.rear_axle.track_width) * 0.5 + 1.0; + /// @note Ego uses the unique_route_lanelets get from Autoware, instead of the current lanelet_id + /// value from EntityStatus, therefore canonicalization has to be done in advance, + /// not inside CanonicalizedEntityStatus const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( status.pose, status.bounding_box, unique_route_lanelets, false, matching_distance, hdmap_utils_ptr_); - status_ = traffic_simulator::CanonicalizedEntityStatus(status, canonicalized_lanelet_pose); + status_.set(traffic_simulator::CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); setAutowareStatus(); } -auto EgoEntitySimulation::updateStatus(double current_scenario_time, double step_time) -> void +auto EgoEntitySimulation::updateStatus(const double current_time, const double step_time) -> void { auto status = static_cast(status_); - status.time = std::isnan(current_scenario_time) ? 0 : current_scenario_time; + status.time = std::isnan(current_time) ? 0 : current_time; status.pose = getCurrentPose(); status.action_status.twist = getCurrentTwist(); status.action_status.accel = getCurrentAccel(step_time); 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 a30e039db06..236ffcaf713 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,250 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose 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 88c064e78ba..71dcb942319 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 3.2.0 + 4.2.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 b2b4b409d44..03d1e0e6252 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. **/ @@ -342,6 +370,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; } } @@ -364,5 +393,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 df6e27408e5..d7e37805abd 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,376 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge pull request `#1323 `_ from tier4/fix/spawn_position_of_map_pose diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 7592cc9fbc9..28014db05be 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -146,10 +146,10 @@ class API if (behavior == VehicleBehavior::autoware()) { return entity_manager_ptr_->entityExists(name) or entity_manager_ptr_->spawnEntity( - name, pose, parameters, configuration, node_parameters_); + name, pose, parameters, getCurrentTime(), configuration, node_parameters_); } else { return entity_manager_ptr_->spawnEntity( - name, pose, parameters, behavior); + name, pose, parameters, getCurrentTime(), behavior); } }; @@ -184,7 +184,7 @@ class API { auto register_to_entity_manager = [&]() { return entity_manager_ptr_->spawnEntity( - name, pose, parameters, behavior); + name, pose, parameters, getCurrentTime(), behavior); }; auto register_to_environment_simulator = [&]() { @@ -213,7 +213,8 @@ class API const std::string & model3d = "") { auto register_to_entity_manager = [&]() { - return entity_manager_ptr_->spawnEntity(name, pose, parameters); + return entity_manager_ptr_->spawnEntity( + name, pose, parameters, getCurrentTime()); }; auto register_to_environment_simulator = [&]() { @@ -250,8 +251,7 @@ class API const std::string & name, const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void; auto setEntityStatus( - const std::string & name, - const std::optional & canonicalized_lanelet_pose, + const std::string & name, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status = helper::constructActionStatus()) -> void; auto setEntityStatus( @@ -268,6 +268,10 @@ class API std::optional getTimeHeadway(const std::string & from, const std::string & to); + auto attachImuSensor( + const std::string &, const simulation_api_schema::ImuSensorConfiguration & configuration) + -> bool; + bool attachPseudoTrafficLightDetector( const simulation_api_schema::PseudoTrafficLightDetectorConfiguration &); @@ -331,6 +335,8 @@ class API const bool allow_spawn_outside_lane = false, const bool require_footprint_fitting = false, const bool random_orientation = false, std::optional random_seed = std::nullopt) -> void; + auto getEntity(const std::string & name) const -> std::shared_ptr; + // clang-format off #define FORWARD_TO_ENTITY_MANAGER(NAME) \ /*! \ @@ -360,10 +366,9 @@ class API FORWARD_TO_ENTITY_MANAGER(getCurrentAction); FORWARD_TO_ENTITY_MANAGER(getCurrentTwist); FORWARD_TO_ENTITY_MANAGER(getEgoName); - FORWARD_TO_ENTITY_MANAGER(getEntity); FORWARD_TO_ENTITY_MANAGER(getEntityNames); FORWARD_TO_ENTITY_MANAGER(getEntityStatus); - FORWARD_TO_ENTITY_MANAGER(getEntityStatusBeforeUpdate); + FORWARD_TO_ENTITY_MANAGER(getCanonicalizedStatusBeforeUpdate); FORWARD_TO_ENTITY_MANAGER(getHdmapUtils); FORWARD_TO_ENTITY_MANAGER(getLinearJerk); FORWARD_TO_ENTITY_MANAGER(getStandStillDuration); 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 1b09d5b3c3f..0d4847f7d5c 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 @@ -41,7 +41,7 @@ class BehaviorPluginBase public: virtual ~BehaviorPluginBase() = default; virtual void configure(const rclcpp::Logger & logger) = 0; - virtual void update(double current_time, double step_time) = 0; + virtual auto update(const double current_time, const double step_time) -> void = 0; virtual const std::string & getCurrentAction() const = 0; #define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \ @@ -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(TrafficLightManager, "traffic_light_manager", std::shared_ptr) - DEFINE_GETTER_SETTER(UpdatedStatus, "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/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp index 0665ccd8f3a..da3293fc67e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp @@ -16,6 +16,7 @@ #define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ #include +#include #include #include #include @@ -30,8 +31,7 @@ auto makeUpdatedStatus( traffic_simulator_msgs::msg::PolylineTrajectory &, const traffic_simulator_msgs::msg::BehaviorParameter &, const std::shared_ptr &, double, double, - std::optional target_speed = std::nullopt) - -> std::optional; + std::optional target_speed = std::nullopt) -> std::optional; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp index 79d3d516699..95df82b7990 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/longitudinal_speed_planning.hpp @@ -26,7 +26,7 @@ namespace longitudinal_speed_planning class LongitudinalSpeedPlanner { public: - explicit LongitudinalSpeedPlanner(double step_time, const std::string & entity); + explicit LongitudinalSpeedPlanner(const double step_time, const std::string & entity); auto getDynamicStates( double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &, const geometry_msgs::msg::Twist & current_twist, 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 53897ac0add..68a781808a3 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 @@ -17,60 +17,75 @@ #include #include +#include #include namespace traffic_simulator { using EntityStatus = traffic_simulator_msgs::msg::EntityStatus; +using EntityType = traffic_simulator_msgs::msg::EntityType; +using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; inline namespace entity_status { class CanonicalizedEntityStatus { public: - explicit CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils); explicit CanonicalizedEntityStatus( const EntityStatus & may_non_canonicalized_entity_status, const std::optional & canonicalized_lanelet_pose); - explicit CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils, - const lanelet::Ids & route_lanelets); explicit CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj); explicit operator EntityStatus() const noexcept { return entity_status_; } - auto operator=(const CanonicalizedEntityStatus & obj) -> CanonicalizedEntityStatus &; + + auto set(const CanonicalizedEntityStatus & status) -> void; + auto set( + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void; + 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 getName() const noexcept -> const std::string & { return entity_status_.name; }; - auto getBoundingBox() const noexcept -> traffic_simulator_msgs::msg::BoundingBox; - auto laneMatchingSucceed() const noexcept -> bool; - auto getMapPose() const noexcept -> geometry_msgs::msg::Pose; - auto getLaneletPose() const -> LaneletPose; - auto getLaneletId() const -> lanelet::Id; - auto getLaneletIds() const -> lanelet::Ids; - auto getCanonicalizedLaneletPose() const -> std::optional; + auto getActionStatus() const noexcept -> const traffic_simulator_msgs::msg::ActionStatus &; + + auto getTime() const noexcept -> double; + auto setTime(double) -> void; + + auto getMapPose() const noexcept -> const geometry_msgs::msg::Pose &; + auto setMapPose(const geometry_msgs::msg::Pose & pose) -> void; + + auto getTwist() const noexcept -> const geometry_msgs::msg::Twist &; auto setTwist(const geometry_msgs::msg::Twist & twist) -> void; - auto getTwist() const noexcept -> geometry_msgs::msg::Twist; auto setLinearVelocity(double linear_velocity) -> void; + + auto getAccel() const noexcept -> const geometry_msgs::msg::Accel &; auto setAccel(const geometry_msgs::msg::Accel & accel) -> void; auto setLinearAcceleration(double linear_acceleration) -> void; - auto getAccel() const noexcept -> geometry_msgs::msg::Accel; - auto setLinearJerk(double) -> void; + auto getLinearJerk() const noexcept -> double; - auto setTime(double) -> void; - auto getTime() const noexcept -> double; + auto setLinearJerk(double) -> void; + + auto laneMatchingSucceed() const noexcept -> bool; + auto getLaneletId() const noexcept -> lanelet::Id; + auto getLaneletIds() const noexcept -> lanelet::Ids; + auto getLaneletPose() const noexcept -> const LaneletPose &; + auto getCanonicalizedLaneletPose() const noexcept + -> const std::optional &; + auto getName() const noexcept -> const std::string & { return entity_status_.name; } + auto getType() const noexcept -> const EntityType & { return entity_status_.type; } + auto getSubtype() const noexcept -> const EntitySubtype & { return entity_status_.subtype; } + auto getBoundingBox() const noexcept -> const traffic_simulator_msgs::msg::BoundingBox &; private: - auto canonicalize() -> void; std::optional canonicalized_lanelet_pose_; EntityStatus entity_status_; }; } // namespace entity_status - -auto isSameLaneletId(const CanonicalizedEntityStatus &, const CanonicalizedEntityStatus &) -> bool; -auto isSameLaneletId(const CanonicalizedEntityStatus &, const lanelet::Id lanelet_id) -> bool; - +auto isSameLaneletId( + const CanonicalizedEntityStatus & first_status, const CanonicalizedEntityStatus & second_status) + -> bool; +auto isSameLaneletId(const CanonicalizedEntityStatus & status, const lanelet::Id lanelet_id) + -> bool; } // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__DATA_TYPE__ENTITY_STATUS_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp index a22d43a7afd..a1d9d83b7bf 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp @@ -37,6 +37,7 @@ class CanonicalizedLaneletPose CanonicalizedLaneletPose & operator=(const CanonicalizedLaneletPose & obj); explicit operator LaneletPose() const noexcept { return lanelet_pose_; } explicit operator geometry_msgs::msg::Pose() const noexcept { return map_pose_; } + auto getLaneletPose() const -> const LaneletPose & { return lanelet_pose_; } auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, 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 8575ee3588e..dbe2668f758 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -69,7 +69,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; @@ -78,8 +78,6 @@ class EgoEntity : public VehicleEntity auto getEntityStatus(const double, const double) const -> const CanonicalizedEntityStatus; - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override; - auto getEntityTypename() const -> const std::string & override; auto getObstacle() -> std::optional override; @@ -88,6 +86,8 @@ class EgoEntity : public VehicleEntity auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; + auto updateFieldOperatorApplication() const -> void; + void onUpdate(double current_time, double step_time) override; void requestAcquirePosition(const CanonicalizedLaneletPose &) override; @@ -133,6 +133,18 @@ class EgoEntity : public VehicleEntity auto setVelocityLimit(double) -> void override; auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override; + + template + auto setStatus(Ts &&... xs) + { + if (status_->getTime() > 0 && not isControlledBySimulator()) { + THROW_SEMANTIC_ERROR( + "You cannot set entity status to the ego vehicle named ", std::quoted(status_->getName()), + " after starting scenario."); + } else { + EntityBase::setStatus(std::forward(xs)...); + } + } }; } // namespace entity } // namespace traffic_simulator 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 ac53c791a75..c29eddf5722 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -71,17 +71,18 @@ class EntityBase */ \ /* */ auto get##NAME() const noexcept->TYPE { return RETURN_VARIABLE; } - DEFINE_GETTER(BoundingBox, traffic_simulator_msgs::msg::BoundingBox, static_cast(getStatus()).bounding_box) - DEFINE_GETTER(CurrentAccel, geometry_msgs::msg::Accel, static_cast(getStatus()).action_status.accel) - DEFINE_GETTER(CurrentTwist, geometry_msgs::msg::Twist, static_cast(getStatus()).action_status.twist) - DEFINE_GETTER(DynamicConstraints, traffic_simulator_msgs::msg::DynamicConstraints, getBehaviorParameter().dynamic_constraints) - DEFINE_GETTER(EntityStatusBeforeUpdate, const CanonicalizedEntityStatus &, status_before_update_) - DEFINE_GETTER(EntitySubtype, traffic_simulator_msgs::msg::EntitySubtype, static_cast(getStatus()).subtype) - DEFINE_GETTER(LinearJerk, double, static_cast(getStatus()).action_status.linear_jerk) - DEFINE_GETTER(MapPose, geometry_msgs::msg::Pose, static_cast(getStatus()).pose) - DEFINE_GETTER(StandStillDuration, double, stand_still_duration_) - DEFINE_GETTER(Status, const CanonicalizedEntityStatus &, status_) - DEFINE_GETTER(TraveledDistance, double, traveled_distance_) + 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(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(StandStillDuration, double, stand_still_duration_) + DEFINE_GETTER(TraveledDistance, double, traveled_distance_) // clang-format on #undef DEFINE_GETTER @@ -92,8 +93,7 @@ class EntityBase */ \ /* */ auto FUNCTION_NAME() const->bool { return BOOL_VARIABLE; } - DEFINE_CHECK_FUNCTION(isNpcLogicStarted, npc_logic_started_) - DEFINE_CHECK_FUNCTION(laneMatchingSucceed, status_.laneMatchingSucceed()) + DEFINE_CHECK_FUNCTION(laneMatchingSucceed, status_->laneMatchingSucceed()) // clang-format on #undef DEFINE_CHECK_FUNCTION @@ -106,8 +106,6 @@ class EntityBase virtual auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & = 0; - virtual auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & = 0; - virtual auto getEntityTypename() const -> const std::string & = 0; virtual auto getGoalPoses() -> std::vector = 0; @@ -117,9 +115,6 @@ class EntityBase /* */ auto getCanonicalizedLaneletPose(double matching_distance) const -> std::optional; - /* */ auto getMapPoseFromRelativePose(const geometry_msgs::msg::Pose &) const - -> geometry_msgs::msg::Pose; - virtual auto getMaxAcceleration() const -> double = 0; virtual auto getMaxDeceleration() const -> double = 0; @@ -132,9 +127,9 @@ class EntityBase virtual auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray = 0; - virtual void onUpdate(double current_time, double step_time); + virtual auto onUpdate(const double current_time, const double step_time) -> void; - virtual void onPostUpdate(double current_time, double step_time); + virtual auto onPostUpdate(const double current_time, const double step_time) -> void; /* */ void resetDynamicConstraints(); @@ -194,7 +189,11 @@ class EntityBase /* */ void setOtherStatus(const std::unordered_map &); - virtual auto setStatus(const CanonicalizedEntityStatus &) -> void; + virtual auto setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void; + + virtual auto setStatus(const EntityStatus & status) -> void; + + /* */ auto setCanonicalizedStatus(const CanonicalizedEntityStatus &) -> void; virtual auto setLinearAcceleration(const double linear_acceleration) -> void; @@ -215,9 +214,9 @@ class EntityBase /* */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void; - /* */ auto setLinearJerk(const double liner_jerk) -> void; + /* */ auto setAction(const std::string & action) -> void; - virtual void startNpcLogic(const double current_time); + /* */ auto setLinearJerk(const double liner_jerk) -> void; /* */ void stopAtCurrentPosition(); @@ -245,14 +244,13 @@ class EntityBase bool verbose; protected: - CanonicalizedEntityStatus status_; + std::shared_ptr status_; CanonicalizedEntityStatus status_before_update_; std::shared_ptr hdmap_utils_ptr_; std::shared_ptr traffic_light_manager_; - bool npc_logic_started_ = false; double stand_still_duration_ = 0.0; double traveled_distance_ = 0.0; double prev_job_duration_ = 0.0; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index b90788d5437..70ce1b78f40 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -85,10 +85,6 @@ class EntityManager std::unordered_map> entities_; - double step_time_; - - double current_time_; - bool npc_logic_started_; using EntityStatusWithTrajectoryArray = @@ -158,7 +154,6 @@ class EntityManager broadcaster_(node), base_link_broadcaster_(node), clock_ptr_(node->get_clock()), - current_time_(std::numeric_limits::quiet_NaN()), npc_logic_started_(false), entity_status_array_pub_ptr_(rclcpp::create_publisher( node, "entity/status", EntityMarkerQoS(), @@ -263,33 +258,32 @@ class EntityManager static_assert(true, "") // clang-format on + FORWARD_TO_ENTITY(activateOutOfRangeJob, ); FORWARD_TO_ENTITY(asFieldOperatorApplication, const); + FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(get2DPolygon, const); FORWARD_TO_ENTITY(getBehaviorParameter, const); FORWARD_TO_ENTITY(getBoundingBox, const); + FORWARD_TO_ENTITY(getCanonicalizedStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getCurrentAccel, const); - FORWARD_TO_ENTITY(getCurrentAction, const); FORWARD_TO_ENTITY(getCurrentTwist, const); FORWARD_TO_ENTITY(getDefaultMatchingDistanceForLaneletPoseCalculation, const); - FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getEntityType, const); - FORWARD_TO_ENTITY(reachPosition, const); FORWARD_TO_ENTITY(getEntityTypename, const); FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getRouteLanelets, const); FORWARD_TO_ENTITY(getStandStillDuration, const); FORWARD_TO_ENTITY(getTraveledDistance, const); - FORWARD_TO_ENTITY(laneMatchingSucceed, const); - FORWARD_TO_ENTITY(activateOutOfRangeJob, ); - FORWARD_TO_ENTITY(cancelRequest, ); FORWARD_TO_ENTITY(isControlledBySimulator, ); + FORWARD_TO_ENTITY(laneMatchingSucceed, const); + FORWARD_TO_ENTITY(reachPosition, const); FORWARD_TO_ENTITY(requestAcquirePosition, ); FORWARD_TO_ENTITY(requestAssignRoute, ); + FORWARD_TO_ENTITY(requestClearRoute, ); FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestSynchronize, ); FORWARD_TO_ENTITY(requestWalkStraight, ); - FORWARD_TO_ENTITY(requestClearRoute, ); FORWARD_TO_ENTITY(setAcceleration, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); FORWARD_TO_ENTITY(setAccelerationRateLimit, ); @@ -302,29 +296,18 @@ class EntityManager FORWARD_TO_ENTITY(setMapPose, ); FORWARD_TO_ENTITY(setTwist, ); FORWARD_TO_ENTITY(setVelocityLimit, ); + FORWARD_TO_ENTITY(requestSpeedChange, ); #undef FORWARD_TO_ENTITY + auto getCurrentAction(const std::string & name) const -> std::string; + visualization_msgs::msg::MarkerArray makeDebugMarker() const; bool trafficLightsChanged(); - void requestSpeedChange(const std::string & name, double target_speed, bool continuous); - - void requestSpeedChange( - const std::string & name, const double target_speed, const speed_change::Transition transition, - const speed_change::Constraint constraint, const bool continuous); - - void requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, - bool continuous); - - void requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, - const speed_change::Transition transition, const speed_change::Constraint constraint, - const bool continuous); - - auto updateNpcLogic(const std::string & name) -> const CanonicalizedEntityStatus &; + auto updateNpcLogic(const std::string & name, const double current_time, const double step_time) + -> const CanonicalizedEntityStatus &; void broadcastEntityTransform(); @@ -338,14 +321,12 @@ class EntityManager bool entityExists(const std::string & name); - auto getCurrentTime() const noexcept -> double; - auto getEntityNames() const -> const std::vector; auto getEntity(const std::string & name) const -> std::shared_ptr; - auto getEntityStatus(const std::string & name) const -> CanonicalizedEntityStatus; + auto getEntityStatus(const std::string & name) const -> const CanonicalizedEntityStatus &; auto getHdmapUtils() -> const std::shared_ptr &; @@ -357,8 +338,6 @@ class EntityManager auto getPedestrianParameters(const std::string & name) const -> const traffic_simulator_msgs::msg::PedestrianParameters &; - auto getStepTime() const noexcept -> double; - auto getVehicleParameters(const std::string & name) const -> const traffic_simulator_msgs::msg::VehicleParameters &; @@ -379,7 +358,7 @@ class EntityManager } else { std::vector poses; for (const auto & lanelet_pose : getGoalPoses(name)) { - poses.push_back(toMapPose(lanelet_pose)); + poses.push_back(pose::toMapPose(lanelet_pose)); } return poses; } @@ -413,13 +392,12 @@ class EntityManager */ void resetBehaviorPlugin(const std::string & name, const std::string & behavior_plugin_name); - auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus & status) -> void; - void setVerbose(const bool verbose); template auto spawnEntity( - const std::string & name, const Pose & pose, const Parameters & parameters, Ts &&... xs) + const std::string & name, const Pose & pose, const Parameters & parameters, + const double current_time, Ts &&... xs) { static_assert( std::disjunction< @@ -448,7 +426,7 @@ class EntityManager } entity_status.subtype = parameters.subtype; - entity_status.time = getCurrentTime(); + entity_status.time = current_time; entity_status.name = name; entity_status.bounding_box = parameters.bounding_box; entity_status.action_status = traffic_simulator_msgs::msg::ActionStatus(); @@ -477,11 +455,11 @@ class EntityManager "LaneletPose is not supported type as pose argument. Only CanonicalizedLaneletPose and " "msg::Pose are supported as pose argument of EntityManager::spawnEntity()."); } else if constexpr (std::is_same_v, CanonicalizedLaneletPose>) { - entity_status.pose = toMapPose(pose); + entity_status.pose = pose::toMapPose(pose); return CanonicalizedEntityStatus(entity_status, pose); } else if constexpr (std::is_same_v, geometry_msgs::msg::Pose>) { entity_status.pose = pose; - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( pose, parameters.bounding_box, include_crosswalk, matching_distance, hdmap_utils_ptr_); return CanonicalizedEntityStatus(entity_status, canonicalized_lanelet_pose); } @@ -494,9 +472,6 @@ class EntityManager success) { // FIXME: this ignores V2I traffic lights iter->second->setTrafficLightManager(conventional_traffic_light_manager_ptr_); - if (npc_logic_started_ && not is(name)) { - iter->second->startNpcLogic(getCurrentTime()); - } return success; } else { THROW_SEMANTIC_ERROR("Entity ", std::quoted(name), " is already exists."); @@ -520,9 +495,9 @@ class EntityManager void updateHdmapMarker(); - void startNpcLogic(const double current_time); + auto startNpcLogic(const double current_time) -> void; - auto isNpcLogicStarted() const { return npc_logic_started_; } + auto isNpcLogicStarted() const -> bool { return npc_logic_started_; } }; } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp index 6617144d89b..27ac070dc13 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/misc_object_entity.hpp @@ -38,13 +38,6 @@ class MiscObjectEntity : public EntityBase auto getDefaultDynamicConstraints() const -> const traffic_simulator_msgs::msg::DynamicConstraints & override; - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override - { - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; - return type; - } - auto getEntityTypename() const -> const std::string & override { static const std::string result = "MiscObjectEntity"; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp index 6c5dec63a36..ce36aa648b0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -66,11 +66,9 @@ class PedestrianEntity : public EntityBase void appendDebugMarker(visualization_msgs::msg::MarkerArray & marker_array) override; - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override; - auto getEntityTypename() const -> const std::string & override; - void onUpdate(double current_time, double step_time) override; + auto onUpdate(const double current_time, const double step_time) -> void override; void requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) override; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp index a0fa023d5b5..d3b6b53567c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -84,8 +84,6 @@ class VehicleEntity : public EntityBase auto getMaxDeceleration() const -> double override; - auto getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & override; - auto getEntityTypename() const -> const std::string & override; auto getGoalPoses() -> std::vector override; @@ -96,7 +94,7 @@ class VehicleEntity : public EntityBase auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override; - void onUpdate(double current_time, double step_time) override; + auto onUpdate(const double current_time, const double step_time) -> void override; void requestAcquirePosition(const CanonicalizedLaneletPose &); 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 ae4419b81ac..b3a60eeb163 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 3.2.0 + 4.2.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 fb71cfcf8ca..d02bbfc7c04 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -99,20 +99,22 @@ auto API::respawn( } } +auto API::getEntity(const std::string & name) const -> std::shared_ptr +{ + return entity_manager_ptr_->getEntity(name); +} + auto API::setEntityStatus( - const std::string & name, - const std::optional & canonicalized_lanelet_pose, + const std::string & name, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { if (const auto entity = getEntity(name)) { - auto status = static_cast(entity->getStatus()); + auto status = static_cast(entity->getCanonicalizedStatus()); status.action_status = action_status; - if (canonicalized_lanelet_pose) { - status.pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); - status.lanelet_pose_valid = true; - } - entity->setStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); + status.pose = static_cast(canonicalized_lanelet_pose); + status.lanelet_pose = static_cast(canonicalized_lanelet_pose); + status.lanelet_pose_valid = true; + entity->setCanonicalizedStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } else { THROW_SIMULATION_ERROR("Cannot set entity \"", name, "\" status - such entity does not exist."); } @@ -121,21 +123,7 @@ auto API::setEntityStatus( auto API::setEntityStatus(const std::string & name, const EntityStatus & status) -> void { if (const auto entity = getEntity(name)) { - if (status.lanelet_pose_valid) { - const auto canonicalized_lanelet_pose = - canonicalize(status.lanelet_pose, entity_manager_ptr_->getHdmapUtils()); - entity->setStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); - } else { - const auto include_crosswalk = [](const auto & entity_type) { - return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) || - (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type); - }(entity->getEntityType()); - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - status.pose, entity->getBoundingBox(), include_crosswalk, - entity->getDefaultMatchingDistanceForLaneletPoseCalculation(), - entity_manager_ptr_->getHdmapUtils()); - entity->setStatus(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); - } + entity->setStatus(status); } else { THROW_SIMULATION_ERROR("Cannot set entity \"", name, "\" status - such entity does not exist."); } @@ -147,7 +135,8 @@ std::optional API::getTimeHeadway( { if (auto from_entity = getEntity(from_entity_name); from_entity) { if (auto to_entity = getEntity(to_entity_name); to_entity) { - if (auto relative_pose = relativePose(from_entity->getMapPose(), to_entity->getMapPose()); + if (auto relative_pose = + pose::relativePose(from_entity->getMapPose(), to_entity->getMapPose()); relative_pose && relative_pose->position.x <= 0) { const double time_headway = (relative_pose->position.x * -1) / getCurrentTwist(to_entity_name).linear.x; @@ -162,8 +151,16 @@ auto API::setEntityStatus( const std::string & name, const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - setEntityStatus( - name, canonicalize(lanelet_pose, entity_manager_ptr_->getHdmapUtils()), action_status); + if ( + const auto canonicalized_lanelet_pose = + pose::canonicalize(lanelet_pose, entity_manager_ptr_->getHdmapUtils())) { + setEntityStatus(name, canonicalized_lanelet_pose.value(), action_status); + } else { + std::stringstream ss; + ss << "Status can not be set. lanelet pose: " << lanelet_pose + << " cannot be canonicalized for "; + THROW_SEMANTIC_ERROR(ss.str(), " entity named: ", std::quoted(name), "."); + } } auto API::setEntityStatus( @@ -171,7 +168,7 @@ auto API::setEntityStatus( const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { if (const auto entity = getEntity(name)) { - EntityStatus status = static_cast(entity->getStatus()); + EntityStatus status = static_cast(entity->getCanonicalizedStatus()); status.pose = map_pose; status.action_status = action_status; setEntityStatus(name, status); @@ -187,7 +184,7 @@ auto API::setEntityStatus( { if (const auto reference_entity = getEntity(reference_entity_name)) { setEntityStatus( - name, transformRelativePoseToGlobal(reference_entity->getMapPose(), relative_pose), + name, pose::transformRelativePoseToGlobal(reference_entity->getMapPose(), relative_pose), action_status); } else { THROW_SIMULATION_ERROR( @@ -208,6 +205,14 @@ auto API::setEntityStatus( setEntityStatus(name, reference_entity_name, relative_pose, action_status); } +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) { @@ -296,8 +301,9 @@ bool API::updateEntitiesStatusInSim() simulation_api_schema::UpdateEntityStatusRequest req; req.set_npc_logic_started(entity_manager_ptr_->isNpcLogicStarted()); for (const auto & entity_name : entity_manager_ptr_->getEntityNames()) { - auto entity_status = entity_manager_ptr_->getEntityStatus(entity_name); - simulation_interface::toProto(static_cast(entity_status), *req.add_status()); + const auto entity_status = + static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); + simulation_interface::toProto(entity_status, *req.add_status()); if (entity_manager_ptr_->is(entity_name)) { req.set_overwrite_ego_status(entity_manager_ptr_->isControlledBySimulator(entity_name)); } @@ -306,17 +312,18 @@ bool API::updateEntitiesStatusInSim() simulation_api_schema::UpdateEntityStatusResponse res; if (auto res = zeromq_client_.call(req); res.result().success()) { for (const auto & res_status : res.status()) { - auto name = res_status.name(); - auto entity_status = static_cast(entity_manager_ptr_->getEntityStatus(name)); + auto entity_name = res_status.name(); + auto entity_status = + static_cast(entity_manager_ptr_->getEntityStatus(entity_name)); simulation_interface::toMsg(res_status.pose(), entity_status.pose); simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); - if (entity_manager_ptr_->is(name)) { - setMapPose(name, entity_status.pose); - setTwist(name, entity_status.action_status.twist); - setAcceleration(name, entity_status.action_status.accel); + if (entity_manager_ptr_->is(entity_name)) { + setMapPose(entity_name, entity_status.pose); + setTwist(entity_name, entity_status.action_status.twist); + setAcceleration(entity_name, entity_status.action_status.accel); } else { - setEntityStatus(name, entity_status); + setEntityStatus(entity_name, entity_status); } } return true; @@ -347,16 +354,18 @@ 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; } void API::startNpcLogic() { - if (isNpcLogicStarted()) { + if (entity_manager_ptr_->isNpcLogicStarted()) { THROW_SIMULATION_ERROR("NPC logics are already started."); + } else { + clock_.start(); + entity_manager_ptr_->startNpcLogic(getCurrentTime()); } - clock_.start(); - entity_manager_ptr_->startNpcLogic(getCurrentTime()); } void API::requestLaneChange(const std::string & name, const lanelet::Id & lanelet_id) diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index c27cabdd538..da2ced4710c 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -47,9 +47,8 @@ auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus & entity_status, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, - const std::shared_ptr & hdmap_utils, double step_time, - double matching_distance, std::optional target_speed) - -> std::optional + const std::shared_ptr & hdmap_utils, const double step_time, + double matching_distance, std::optional target_speed) -> std::optional { using math::arithmetic::isApproximatelyEqualTo; using math::arithmetic::isDefinitelyLessThan; @@ -85,31 +84,6 @@ auto makeUpdatedStatus( return hypot(from, to); }; - auto fill_lanelet_data_and_adjust_orientation = - [&](traffic_simulator_msgs::msg::EntityStatus & status) { - if (const auto lanelet_pose = - hdmap_utils->toLaneletPose(status.pose, status.bounding_box, false, matching_distance); - lanelet_pose) { - status.lanelet_pose = lanelet_pose.value(); - const CatmullRomSpline spline(hdmap_utils->getCenterPoints(status.lanelet_pose.lanelet_id)); - const auto lanelet_quaternion = spline.getPose(status.lanelet_pose.s, true).orientation; - const auto lanelet_rpy = math::geometry::convertQuaternionToEulerAngle(lanelet_quaternion); - const auto entity_rpy = - math::geometry::convertQuaternionToEulerAngle(status.pose.orientation); - // adjust orientation in EntityStatus.pose (only pitch) and in EntityStatus.LaneletPose - status.pose.orientation = math::geometry::convertEulerAngleToQuaternion( - geometry_msgs::build() - .x(entity_rpy.x) - .y(lanelet_rpy.y) - .z(entity_rpy.z)); - status.lanelet_pose.rpy = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(lanelet_quaternion, status.pose.orientation)); - status.lanelet_pose_valid = true; - } else { - status.lanelet_pose_valid = false; - } - }; - auto discard_the_front_waypoint_and_recurse = [&]() { /* The OpenSCENARIO standard does not define the behavior when the value of @@ -595,13 +569,6 @@ auto makeUpdatedStatus( } }(); - /* - After the step, i.e. movement in the direction of designed_velocity, it is necessary to adjust - the pose of the entity to the lane - if possible, the pitch orientation may be - changed as a result because the slope of the lane may be different - */ - fill_lanelet_data_and_adjust_orientation(updated_status); - updated_status.action_status.twist.linear.x = norm(desired_velocity); updated_status.action_status.twist.linear.y = 0; @@ -623,6 +590,8 @@ auto makeUpdatedStatus( updated_status.time = entity_status.time + step_time; + updated_status.lanelet_pose_valid = false; + return updated_status; } } diff --git a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp index d2efc60ff89..0fc3c13a0e2 100644 --- a/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp +++ b/simulation/traffic_simulator/src/behavior/longitudinal_speed_planning.cpp @@ -23,7 +23,8 @@ namespace traffic_simulator { namespace longitudinal_speed_planning { -LongitudinalSpeedPlanner::LongitudinalSpeedPlanner(double step_time, const std::string & entity) +LongitudinalSpeedPlanner::LongitudinalSpeedPlanner( + const double step_time, const std::string & entity) : step_time(step_time), entity(entity) { } diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 57869c33917..7bb9052dcea 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -25,47 +25,6 @@ CanonicalizedEntityStatus::CanonicalizedEntityStatus( const std::optional & canonicalized_lanelet_pose) : canonicalized_lanelet_pose_{canonicalized_lanelet_pose}, entity_status_{may_non_canonicalized_entity_status} -{ - canonicalize(); -} - -/// @todo this constructor will be removed (after adaptation of behavior_tree) -CanonicalizedEntityStatus::CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils) -: canonicalized_lanelet_pose_{may_non_canonicalized_entity_status.lanelet_pose_valid?std::optional(CanonicalizedLaneletPose( - may_non_canonicalized_entity_status.lanelet_pose, hdmap_utils)):std::nullopt}, - entity_status_{may_non_canonicalized_entity_status} -{ - canonicalize(); -} - -/// @todo this constructor will be removed (after adaptation of behavior_tree) -CanonicalizedEntityStatus::CanonicalizedEntityStatus( - const EntityStatus & may_non_canonicalized_entity_status, - const std::shared_ptr & hdmap_utils, const lanelet::Ids & route_lanelets) -: canonicalized_lanelet_pose_{may_non_canonicalized_entity_status.lanelet_pose_valid?std::optional(CanonicalizedLaneletPose( - may_non_canonicalized_entity_status.lanelet_pose, route_lanelets, hdmap_utils )):std::nullopt}, - entity_status_{may_non_canonicalized_entity_status} -{ - canonicalize(); -} - -CanonicalizedEntityStatus::CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj) -: canonicalized_lanelet_pose_(obj.canonicalized_lanelet_pose_), - entity_status_(static_cast(obj)) -{ -} - -CanonicalizedEntityStatus & CanonicalizedEntityStatus::operator=( - const CanonicalizedEntityStatus & obj) -{ - this->canonicalized_lanelet_pose_ = obj.canonicalized_lanelet_pose_; - this->entity_status_ = obj.entity_status_; - return *this; -} - -auto CanonicalizedEntityStatus::canonicalize() -> void { assert(entity_status_.lanelet_pose_valid == canonicalized_lanelet_pose_.has_value()); if (canonicalized_lanelet_pose_) { @@ -75,7 +34,7 @@ auto CanonicalizedEntityStatus::canonicalize() -> void The position in Oz axis and orientation based on LaneletPose are rewritten to the used msg::Pose (map_pose) since such adjustment relative to the lanelet is necessary, The position in Ox and Oy axis is not rewritten because the map_pose retrieved via - lanelet_pose = toCanonicalizedLaneletPose(map_pose), then map_pose = toMapPose(lanelet_pose) + lanelet_pose = pose::toCanonicalizedLaneletPose(map_pose), then map_pose pose::toMapPose(lanelet_pose) can be slightly different from the original one (especially if the entity changes lane). */ const auto map_pose_based_on_lanelet_pose = @@ -88,43 +47,102 @@ auto CanonicalizedEntityStatus::canonicalize() -> void } } +CanonicalizedEntityStatus::CanonicalizedEntityStatus(const CanonicalizedEntityStatus & obj) +: canonicalized_lanelet_pose_(obj.canonicalized_lanelet_pose_), + entity_status_(static_cast(obj)) +{ +} + +auto CanonicalizedEntityStatus::set(const CanonicalizedEntityStatus & status) -> void +{ + assert(getType() == status.getType()); + assert(getSubtype() == status.getSubtype()); + assert(getName() == status.getName()); + assert(getBoundingBox() == status.getBoundingBox()); + entity_status_ = status.entity_status_; + canonicalized_lanelet_pose_ = status.canonicalized_lanelet_pose_; +} + +auto CanonicalizedEntityStatus::set( + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void +{ + const auto include_crosswalk = + getType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN || + getType().type == traffic_simulator_msgs::msg::EntityType::MISC_OBJECT; + + std::optional canonicalized_lanelet_pose; + if (status.lanelet_pose_valid) { + canonicalized_lanelet_pose = pose::canonicalize(status.lanelet_pose, hdmap_utils_ptr); + } else { + // prefer the current lanelet + canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + status.pose, getBoundingBox(), lanelet_ids, include_crosswalk, matching_distance, + hdmap_utils_ptr); + } + set(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); +} + +auto CanonicalizedEntityStatus::set( + const EntityStatus & status, const double matching_distance, + const std::shared_ptr & hdmap_utils_ptr) -> void +{ + 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::laneMatchingSucceed() const noexcept -> bool { return canonicalized_lanelet_pose_.has_value(); } auto CanonicalizedEntityStatus::getBoundingBox() const noexcept - -> traffic_simulator_msgs::msg::BoundingBox + -> const traffic_simulator_msgs::msg::BoundingBox & { return entity_status_.bounding_box; } -auto CanonicalizedEntityStatus::getMapPose() const noexcept -> geometry_msgs::msg::Pose +auto CanonicalizedEntityStatus::setMapPose(const geometry_msgs::msg::Pose & pose) -> void +{ + entity_status_.pose = pose; +} + +auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_msgs::msg::Pose & { return entity_status_.pose; } -auto CanonicalizedEntityStatus::getLaneletPose() const -> LaneletPose +auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & { if (canonicalized_lanelet_pose_) { - return static_cast(canonicalized_lanelet_pose_.value()); + return canonicalized_lanelet_pose_->getLaneletPose(); } else { THROW_SEMANTIC_ERROR("Target entity status did not matched to lanelet pose."); } } -auto CanonicalizedEntityStatus::getLaneletId() const -> lanelet::Id +auto CanonicalizedEntityStatus::getLaneletId() const noexcept -> lanelet::Id { return getLaneletPose().lanelet_id; } -auto CanonicalizedEntityStatus::getLaneletIds() const -> lanelet::Ids +auto CanonicalizedEntityStatus::getLaneletIds() const noexcept -> lanelet::Ids { return laneMatchingSucceed() ? lanelet::Ids{getLaneletId()} : lanelet::Ids{}; } -auto CanonicalizedEntityStatus::getCanonicalizedLaneletPose() const - -> std::optional +auto CanonicalizedEntityStatus::getCanonicalizedLaneletPose() const noexcept + -> const std::optional & { return canonicalized_lanelet_pose_; } @@ -134,7 +152,7 @@ auto CanonicalizedEntityStatus::setTwist(const geometry_msgs::msg::Twist & twist entity_status_.action_status.twist = twist; } -auto CanonicalizedEntityStatus::getTwist() const noexcept -> geometry_msgs::msg::Twist +auto CanonicalizedEntityStatus::getTwist() const noexcept -> const geometry_msgs::msg::Twist & { return entity_status_.action_status.twist; } @@ -154,7 +172,7 @@ auto CanonicalizedEntityStatus::setLinearAcceleration(double linear_acceleration entity_status_.action_status.accel.linear.x = linear_acceleration; } -auto CanonicalizedEntityStatus::getAccel() const noexcept -> geometry_msgs::msg::Accel +auto CanonicalizedEntityStatus::getAccel() const noexcept -> const geometry_msgs::msg::Accel & { return entity_status_.action_status.accel; } @@ -164,11 +182,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; @@ -179,23 +192,15 @@ auto CanonicalizedEntityStatus::setTime(double time) -> void { entity_status_.ti auto CanonicalizedEntityStatus::getTime() const noexcept -> double { return entity_status_.time; } } // namespace entity_status -auto isSameLaneletId(const CanonicalizedEntityStatus & s0, const CanonicalizedEntityStatus & s1) +auto isSameLaneletId( + const CanonicalizedEntityStatus & first_status, const CanonicalizedEntityStatus & second_status) -> bool { - if (const auto s0_canonicalized_lanelet_pose = s0.getCanonicalizedLaneletPose()) { - if (const auto s1_canonicalized_lanelet_pose = s1.getCanonicalizedLaneletPose()) { - return isSameLaneletId( - s0_canonicalized_lanelet_pose.value(), s1_canonicalized_lanelet_pose.value()); - } - } - THROW_SIMULATION_ERROR("There is no Lanelet pose"); + return first_status.getLaneletId() == second_status.getLaneletId(); } -auto isSameLaneletId(const CanonicalizedEntityStatus & s, const lanelet::Id lanelet_id) -> bool +auto isSameLaneletId(const CanonicalizedEntityStatus & status, const lanelet::Id lanelet_id) -> bool { - if (const auto s_canonicalized_lanelet_pose = s.getCanonicalizedLaneletPose()) { - return isSameLaneletId(s_canonicalized_lanelet_pose.value(), lanelet_id); - } - THROW_SIMULATION_ERROR("There is no Lanelet pose"); + return status.getLaneletId() == lanelet_id; } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index 5153f119076..140ab39bf15 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -30,7 +30,7 @@ CanonicalizedLaneletPose::CanonicalizedLaneletPose( : lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, hdmap_utils)), lanelet_poses_( hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(toMapPose(lanelet_pose_, hdmap_utils)) + map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) { adjustOrientationAndOzPosition(hdmap_utils); } @@ -41,7 +41,7 @@ CanonicalizedLaneletPose::CanonicalizedLaneletPose( : lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, route_lanelets, hdmap_utils)), lanelet_poses_( hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(toMapPose(lanelet_pose_, hdmap_utils)) + map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) { adjustOrientationAndOzPosition(hdmap_utils); } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e9277e81124..7ba999c60b1 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 { @@ -103,13 +106,6 @@ auto EgoEntity::getEntityTypename() const -> const std::string & return result; } -auto EgoEntity::getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & -{ - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::EGO; - return type; -} - auto EgoEntity::getObstacle() -> std::optional { return std::nullopt; @@ -131,39 +127,47 @@ 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 { return field_operator_application->getWaypoints(); } +void EgoEntity::updateFieldOperatorApplication() const +{ + field_operator_application->rethrow(); + field_operator_application->spinSome(); +} + void EgoEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); - if (is_controlled_by_simulator_ && npc_logic_started_) { + if (is_controlled_by_simulator_) { if ( - const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(status_), *polyline_trajectory_, - behavior_parameter_, hdmap_utils_ptr_, step_time, - getDefaultMatchingDistanceForLaneletPoseCalculation(), - target_speed_ ? target_speed_.value() : status_.getTwist().linear.x)) { - // prefer the current lanelet - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - updated_status.value().pose, status_.getBoundingBox(), status_.getLaneletIds(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); - setStatus(CanonicalizedEntityStatus(*updated_status, canonicalized_lanelet_pose)); + const auto non_canonicalized_updated_status = + traffic_simulator::follow_trajectory::makeUpdatedStatus( + static_cast(*status_), *polyline_trajectory_, + behavior_parameter_, hdmap_utils_ptr_, step_time, + getDefaultMatchingDistanceForLaneletPoseCalculation(), + target_speed_ ? target_speed_.value() : status_->getTwist().linear.x)) { + // prefer current lanelet on ss2 side + 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); - - field_operator_application->rethrow(); - field_operator_application->spinSome(); + updateFieldOperatorApplication(); EntityBase::onPostUpdate(current_time, step_time); } @@ -269,15 +273,22 @@ auto EgoEntity::setBehaviorParameter( behavior_parameter_ = behavior_parameter; } -void EgoEntity::requestSpeedChange(double value, bool) +auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void { - target_speed_ = value; - field_operator_application->restrictTargetSpeed(value); + if (status_->getTime() > 0.0) { + THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); + } else { + target_speed_ = value; + field_operator_application->restrictTargetSpeed(value); + } } -void EgoEntity::requestSpeedChange( - const speed_change::RelativeTargetSpeed & /*target_speed*/, bool /*continuous*/) +auto EgoEntity::requestSpeedChange( + const speed_change::RelativeTargetSpeed & /*target_speed*/, bool /*continuous*/) -> void { + THROW_SEMANTIC_ERROR( + "The traffic_simulator's request to set speed to the Ego type entity is for initialization " + "purposes only."); } auto EgoEntity::setVelocityLimit(double value) -> void // @@ -288,13 +299,13 @@ auto EgoEntity::setVelocityLimit(double value) -> void // auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void { - auto status = static_cast(status_); - status.pose = map_pose; - const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets()); - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), unique_route_lanelets, false, + auto entity_status = static_cast(*status_); + entity_status.pose = map_pose; + entity_status.lanelet_pose_valid = false; + // prefer current lanelet on Autoware side + status_->set( + entity_status, helper::getUniqueValues(getRouteLanelets()), getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); - status_ = CanonicalizedEntityStatus(status, canonicalized_lanelet_pose); } } // 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 a607f9baa35..ff4b43a4b0f 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -34,10 +34,9 @@ EntityBase::EntityBase( const std::shared_ptr & hdmap_utils_ptr) : name(name), verbose(true), - status_(entity_status), - status_before_update_(status_), - hdmap_utils_ptr_(hdmap_utils_ptr), - npc_logic_started_(false) + status_(std::make_shared(entity_status)), + status_before_update_(*status_), + hdmap_utils_ptr_(hdmap_utils_ptr) { if (name != static_cast(entity_status).name) { THROW_SIMULATION_ERROR( @@ -67,7 +66,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 @@ -79,8 +78,8 @@ auto EntityBase::getCanonicalizedLaneletPose(double matching_distance) const }(getEntityType()); // prefer the current lanelet - return toCanonicalizedLaneletPose( - status_.getMapPose(), status_.getBoundingBox(), status_.getLaneletIds(), include_crosswalk, + return pose::toCanonicalizedLaneletPose( + status_->getMapPose(), status_->getBoundingBox(), status_->getLaneletIds(), include_crosswalk, matching_distance, hdmap_utils_ptr_); } @@ -97,20 +96,21 @@ auto EntityBase::isTargetSpeedReached(double target_speed) const -> bool auto EntityBase::isTargetSpeedReached(const speed_change::RelativeTargetSpeed & target_speed) const -> bool { - return isTargetSpeedReached(target_speed.getAbsoluteValue(getStatus(), other_status_)); + return isTargetSpeedReached( + target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_)); } -void EntityBase::onUpdate(double /*current_time*/, double step_time) +auto EntityBase::onUpdate(const double /*current_time*/, const double step_time) -> void { job_list_.update(step_time, job::Event::PRE_UPDATE); step_time_ = step_time; - status_before_update_ = status_; + status_before_update_.set(*status_); speed_planner_ = std::make_unique( step_time, name); } -void EntityBase::onPostUpdate(double /*current_time*/, double step_time) +auto EntityBase::onPostUpdate(const double /*current_time*/, const double step_time) -> void { job_list_.update(step_time, job::Event::POST_UPDATE); } @@ -140,7 +140,7 @@ void EntityBase::requestLaneChange( "Source entity does not assigned to lanelet. Please check source entity name : ", name, " exists on lane."); } - reference_lanelet_id = getStatus().getLaneletId(); + reference_lanelet_id = status_->getLaneletId(); } else { if (other_status_.find(target.entity_name) == other_status_.end()) { THROW_SEMANTIC_ERROR( @@ -318,8 +318,8 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration( * @brief Checking if the entity reaches target speed. */ [this, target_speed, acceleration](double) { - double diff = - target_speed.getAbsoluteValue(getStatus(), other_status_) - getCurrentTwist().linear.x; + double diff = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_) - + getCurrentTwist().linear.x; /** * @brief Hard coded parameter, threshold for difference */ @@ -346,7 +346,7 @@ void EntityBase::requestSpeedChangeWithConstantAcceleration( } case speed_change::Transition::STEP: { requestSpeedChange(target_speed, continuous); - setLinearVelocity(target_speed.getAbsoluteValue(getStatus(), other_status_)); + setLinearVelocity(target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_)); break; } } @@ -369,17 +369,19 @@ void EntityBase::requestSpeedChangeWithTimeConstraint( switch (transition) { case speed_change::Transition::LINEAR: { requestSpeedChangeWithTimeConstraint( - target_speed.getAbsoluteValue(getStatus(), other_status_), transition, acceleration_time); + target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_), transition, + acceleration_time); break; } case speed_change::Transition::AUTO: { requestSpeedChangeWithTimeConstraint( - target_speed.getAbsoluteValue(getStatus(), other_status_), transition, acceleration_time); + target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_), transition, + acceleration_time); break; } case speed_change::Transition::STEP: { requestSpeedChange(target_speed, false); - setLinearVelocity(target_speed.getAbsoluteValue(getStatus(), other_status_)); + setLinearVelocity(target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_)); break; } } @@ -464,7 +466,7 @@ void EntityBase::requestSpeedChange( if (other_status_.find(target_speed.reference_entity_name) == other_status_.end()) { return true; } - target_speed_ = target_speed.getAbsoluteValue(getStatus(), other_status_); + target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_); return false; }, [this]() {}, job::Type::LINEAR_VELOCITY, true, job::Event::POST_UPDATE); @@ -478,7 +480,7 @@ void EntityBase::requestSpeedChange( return true; } if (isTargetSpeedReached(target_speed)) { - target_speed_ = target_speed.getAbsoluteValue(getStatus(), other_status_); + target_speed_ = target_speed.getAbsoluteValue(getCanonicalizedStatus(), other_status_); return true; } return false; @@ -526,37 +528,30 @@ void EntityBase::setOtherStatus( other_status_.erase(name); } -auto EntityBase::setStatus(const CanonicalizedEntityStatus & status) -> void +auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void { - auto new_status = static_cast(status); + status_->set( + status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); +} - /* - FIXME: DIRTY HACK!!! +auto EntityBase::setStatus(const EntityStatus & status) -> void +{ + status_->set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); +} - It seems that some operations set an incomplete status without respecting - the original status obtained by getStatus. Below is the code to compensate - for the lack of set status. - */ - new_status.name = name; - new_status.type = getEntityType(); - new_status.subtype = getEntitySubtype(); - new_status.bounding_box = getBoundingBox(); - new_status.action_status.current_action = getCurrentAction(); - status_ = CanonicalizedEntityStatus(new_status, status.getCanonicalizedLaneletPose()); +auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void +{ + status_->set(status); } auto EntityBase::setLinearVelocity(const double linear_velocity) -> void { - auto status = static_cast(getStatus()); - status.action_status.twist.linear.x = linear_velocity; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setLinearVelocity(linear_velocity); } auto EntityBase::setLinearAcceleration(const double linear_acceleration) -> void { - auto status = static_cast(getStatus()); - status.action_status.accel.linear.x = linear_acceleration; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setLinearAcceleration(linear_acceleration); } void EntityBase::setTrafficLightManager( @@ -567,25 +562,21 @@ void EntityBase::setTrafficLightManager( auto EntityBase::setTwist(const geometry_msgs::msg::Twist & twist) -> void { - auto new_status = static_cast(getStatus()); - new_status.action_status.twist = twist; - status_ = CanonicalizedEntityStatus(new_status, status_.getCanonicalizedLaneletPose()); + status_->setTwist(twist); } auto EntityBase::setAcceleration(const geometry_msgs::msg::Accel & accel) -> void { - auto new_status = static_cast(getStatus()); - new_status.action_status.accel = accel; - status_ = CanonicalizedEntityStatus(new_status, status_.getCanonicalizedLaneletPose()); + status_->setAccel(accel); } auto EntityBase::setLinearJerk(const double linear_jerk) -> void { - auto new_status = static_cast(getStatus()); - new_status.action_status.linear_jerk = linear_jerk; - status_ = CanonicalizedEntityStatus(new_status, status_.getCanonicalizedLaneletPose()); + status_->setLinearJerk(linear_jerk); } +auto EntityBase::setAction(const std::string & action) -> void { status_->setAction(action); } + auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void { THROW_SEMANTIC_ERROR( @@ -634,44 +625,30 @@ void EntityBase::activateOutOfRangeJob( [this]() {}, job::Type::OUT_OF_RANGE, true, job::Event::POST_UPDATE); } -void EntityBase::startNpcLogic(const double current_time) -{ - updateEntityStatusTimestamp(current_time); - npc_logic_started_ = true; -} - void EntityBase::stopAtCurrentPosition() { - auto status = static_cast(getStatus()); - status.action_status.twist = geometry_msgs::msg::Twist(); - status.action_status.accel = geometry_msgs::msg::Accel(); - status.action_status.linear_jerk = 0; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setTwist(geometry_msgs::msg::Twist()); + status_->setAccel(geometry_msgs::msg::Accel()); + status_->setLinearJerk(0.0); } void EntityBase::updateEntityStatusTimestamp(const double current_time) { - auto status = static_cast(getStatus()); - status.time = current_time; - setStatus(CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose())); + status_->setTime(current_time); } auto EntityBase::updateStandStillDuration(const double step_time) -> double { - if ( - npc_logic_started_ and - std::abs(getCurrentTwist().linear.x) <= std::numeric_limits::epsilon()) { + if (std::abs(getCurrentTwist().linear.x) <= std::numeric_limits::epsilon()) { return stand_still_duration_ += step_time; + } else { + return stand_still_duration_ = 0.0; } - return stand_still_duration_ = 0.0; } auto EntityBase::updateTraveledDistance(const double step_time) -> double { - if (npc_logic_started_) { - traveled_distance_ += std::abs(getCurrentTwist().linear.x) * step_time; - } - return traveled_distance_; + return traveled_distance_ += std::abs(getCurrentTwist().linear.x) * step_time; } bool EntityBase::reachPosition(const std::string & target_name, const double tolerance) const @@ -717,7 +694,7 @@ auto EntityBase::requestSynchronize( ///@brief Check if the entity has already arrived to the target lanelet. if (reachPosition(entity_target, tolerance)) { - if (this->getStatus().getTwist().linear.x < target_speed + getMaxAcceleration() * step_time_) { + if (getCurrentTwist().linear.x < target_speed + getMaxAcceleration() * step_time_) { } else { RCLCPP_WARN_ONCE( rclcpp::get_logger("traffic_simulator"), @@ -765,7 +742,7 @@ auto EntityBase::requestSynchronize( const auto target_entity_velocity = other_status_.find(target_name)->second.getTwist().linear.x; - const auto entity_velocity = this->getStatus().getTwist().linear.x; + const auto entity_velocity = getCurrentTwist().linear.x; const auto target_entity_arrival_time = (std::abs(target_entity_velocity) > std::numeric_limits::epsilon()) ? target_entity_distance.value() / target_entity_velocity diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index ca1e411ddc0..7b730145ccd 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -147,8 +147,6 @@ bool EntityManager::entityExists(const std::string & name) return entities_.find(name) != std::end(entities_); } -auto EntityManager::getCurrentTime() const noexcept -> double { return current_time_; } - auto EntityManager::getEntityNames() const -> const std::vector { std::vector names{}; @@ -175,17 +173,13 @@ auto EntityManager::getEntity(const std::string & name) const } }; -auto EntityManager::getEntityStatus(const std::string & name) const -> CanonicalizedEntityStatus +auto EntityManager::getEntityStatus(const std::string & name) const + -> const CanonicalizedEntityStatus & { - if (const auto iter = entities_.find(name); iter == entities_.end()) { - THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); + if (const auto entity = getEntity(name)) { + return entity->getCanonicalizedStatus(); } else { - auto entity_status = static_cast(iter->second->getStatus()); - assert(entity_status.name == name && "The entity name in status is different from key!"); - entity_status.action_status.current_action = getCurrentAction(name); - entity_status.time = current_time_; - return CanonicalizedEntityStatus( - entity_status, iter->second->getStatus().getCanonicalizedLaneletPose()); + THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); } } @@ -217,10 +211,11 @@ const std::string EntityManager::getEgoName() const auto EntityManager::getObstacle(const std::string & name) -> std::optional { - if (!npc_logic_started_) { + if (not npc_logic_started_) { return std::nullopt; + } else { + return entities_.at(name)->getObstacle(); } - return entities_.at(name)->getObstacle(); } auto EntityManager::getPedestrianParameters(const std::string & name) const @@ -234,8 +229,6 @@ auto EntityManager::getPedestrianParameters(const std::string & name) const "Please check description of the scenario and entity type of the Entity: " + name); } -auto EntityManager::getStepTime() const noexcept -> double { return step_time_; } - auto EntityManager::getVehicleParameters(const std::string & name) const -> const traffic_simulator_msgs::msg::VehicleParameters & { @@ -250,10 +243,11 @@ auto EntityManager::getVehicleParameters(const std::string & name) const auto EntityManager::getWaypoints(const std::string & name) -> traffic_simulator_msgs::msg::WaypointsArray { - if (!npc_logic_started_) { + if (not npc_logic_started_) { return traffic_simulator_msgs::msg::WaypointsArray(); + } else { + return entities_.at(name)->getWaypoints(); } - return entities_.at(name)->getWaypoints(); } bool EntityManager::isEgoSpawned() const @@ -289,7 +283,7 @@ void EntityManager::requestLaneChange( if (const auto entity = getEntity(name); entity && entity->laneMatchingSucceed()) { if ( const auto target = hdmap_utils_ptr_->getLaneChangeableLaneletId( - entity->getStatus().getLaneletId(), direction)) { + entity->getCanonicalizedStatus().getLaneletId(), direction)) { requestLaneChange(name, target.value()); } } @@ -298,7 +292,7 @@ void EntityManager::requestLaneChange( void EntityManager::resetBehaviorPlugin( const std::string & name, const std::string & behavior_plugin_name) { - const auto status = getEntityStatus(name); + const auto & status = getEntityStatus(name); const auto behavior_parameter = getBehaviorParameter(name); if (is(name)) { THROW_SEMANTIC_ERROR( @@ -310,11 +304,13 @@ void EntityManager::resetBehaviorPlugin( } else if (is(name)) { const auto parameters = getVehicleParameters(name); despawnEntity(name); - spawnEntity(name, status.getMapPose(), parameters, behavior_plugin_name); + spawnEntity( + name, status.getMapPose(), parameters, status.getTime(), behavior_plugin_name); } else if (is(name)) { const auto parameters = getPedestrianParameters(name); despawnEntity(name); - spawnEntity(name, status.getMapPose(), parameters, behavior_plugin_name); + spawnEntity( + name, status.getMapPose(), parameters, status.getTime(), behavior_plugin_name); } else { THROW_SIMULATION_ERROR( "Entity :", name, "is unkown entity type.", "Please contact to developer."); @@ -325,61 +321,23 @@ void EntityManager::resetBehaviorPlugin( setBehaviorParameter(name, behavior_parameter); } -bool EntityManager::trafficLightsChanged() -{ - return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or - v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); -} - -void EntityManager::requestSpeedChange( - const std::string & name, double target_speed, bool continuous) -{ - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); - } - return entities_.at(name)->requestSpeedChange(target_speed, continuous); -} - -void EntityManager::requestSpeedChange( - const std::string & name, const double target_speed, const speed_change::Transition transition, - const speed_change::Constraint constraint, const bool continuous) -{ - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); - } - return entities_.at(name)->requestSpeedChange(target_speed, transition, constraint, continuous); -} - -void EntityManager::requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, bool continuous) +auto EntityManager::getCurrentAction(const std::string & name) const -> std::string { - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); - } - return entities_.at(name)->requestSpeedChange(target_speed, continuous); -} - -void EntityManager::requestSpeedChange( - const std::string & name, const speed_change::RelativeTargetSpeed & target_speed, - const speed_change::Transition transition, const speed_change::Constraint constraint, - const bool continuous) -{ - if (is(name) && getCurrentTime() > 0) { - THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); + if (const auto entity = getEntity(name)) { + if (not npc_logic_started_ and not is(name)) { + return "waiting"; + } else { + return entity->getCurrentAction(); + } + } else { + THROW_SEMANTIC_ERROR("entity : ", name, "does not exist"); } - return entities_.at(name)->requestSpeedChange(target_speed, transition, constraint, continuous); } -auto EntityManager::setEntityStatus( - const std::string & name, const CanonicalizedEntityStatus & status) -> void +bool EntityManager::trafficLightsChanged() { - if (is(name) && getCurrentTime() > 0 && not isControlledBySimulator(name)) { - THROW_SEMANTIC_ERROR( - "You cannot set entity status to the ego vehicle name ", std::quoted(name), - " after starting scenario."); - } else { - entities_.at(name)->setStatus(status); - } + return conventional_traffic_light_manager_ptr_->hasAnyLightChanged() or + v2i_traffic_light_manager_ptr_->hasAnyLightChanged(); } void EntityManager::setVerbose(const bool verbose) @@ -390,21 +348,30 @@ void EntityManager::setVerbose(const bool verbose) } } -auto EntityManager::updateNpcLogic(const std::string & name) -> const CanonicalizedEntityStatus & +auto EntityManager::updateNpcLogic( + const std::string & name, const double current_time, const double step_time) + -> const CanonicalizedEntityStatus & { if (configuration.verbose) { std::cout << "update " << name << " behavior" << std::endl; } - entities_[name]->onUpdate(current_time_, step_time_); - return entities_[name]->getStatus(); + if (const auto entity = getEntity(name)) { + // Update npc completely if logic has started, otherwise update Autoware only - if it is Ego + if (npc_logic_started_) { + entity->onUpdate(current_time, step_time); + } else if (const auto ego_entity = std::dynamic_pointer_cast(entity)) { + ego_entity->updateFieldOperatorApplication(); + } + return entity->getCanonicalizedStatus(); + } else { + THROW_SEMANTIC_ERROR("entity ", std::quoted(name), " does not exist."); + } } void EntityManager::update(const double current_time, const double step_time) { traffic_simulator::helper::StopWatch stop_watch_update( "EntityManager::update", configuration.verbose); - step_time_ = step_time; - current_time_ = current_time; setVerbose(configuration.verbose); if (npc_logic_started_) { conventional_traffic_light_updater_.createTimer( @@ -413,14 +380,14 @@ void EntityManager::update(const double current_time, const double step_time) } std::unordered_map all_status; for (auto && [name, entity] : entities_) { - all_status.emplace(name, entity->getStatus()); + all_status.emplace(name, entity->getCanonicalizedStatus()); } for (auto && [name, entity] : entities_) { entity->setOtherStatus(all_status); } all_status.clear(); for (auto && [name, entity] : entities_) { - all_status.emplace(name, updateNpcLogic(name)); + all_status.emplace(name, updateNpcLogic(name, current_time, step_time)); } for (auto && [name, entity] : entities_) { entity->setOtherStatus(all_status); @@ -439,6 +406,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); @@ -448,7 +416,6 @@ void EntityManager::update(const double current_time, const double step_time) if (configuration.verbose) { stop_watch_update.print(); } - current_time_ += step_time; } void EntityManager::updateHdmapMarker() @@ -463,14 +430,12 @@ void EntityManager::updateHdmapMarker() lanelet_marker_pub_ptr_->publish(markers); } -void EntityManager::startNpcLogic(const double current_time) +auto EntityManager::startNpcLogic(const double current_time) -> void { npc_logic_started_ = true; - current_time_ = current_time; - for ([[maybe_unused]] auto && [name, entity] : entities_) { - entity->startNpcLogic(current_time_); + entity->updateEntityStatusTimestamp(current_time); } } } // namespace entity diff --git a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp index 94e413348cb..1f05561e4ea 100644 --- a/simulation/traffic_simulator/src/entity/misc_object_entity.cpp +++ b/simulation/traffic_simulator/src/entity/misc_object_entity.cpp @@ -26,27 +26,19 @@ MiscObjectEntity::MiscObjectEntity( { } -void MiscObjectEntity::onUpdate(double, double step_time) +auto MiscObjectEntity::onUpdate(const double /*current_time*/, const double step_time) -> void { - auto status = static_cast(status_); - status.action_status.twist = geometry_msgs::msg::Twist(); - status.action_status.accel = geometry_msgs::msg::Accel(); - status.action_status.linear_jerk = 0; - status.action_status.current_action = "static"; - status_ = CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose()); - status_before_update_ = CanonicalizedEntityStatus(status, status_.getCanonicalizedLaneletPose()); - if (npc_logic_started_) { - updateStandStillDuration(step_time); - } + setTwist(geometry_msgs::msg::Twist()); + setAcceleration(geometry_msgs::msg::Accel()); + setLinearJerk(0.0); + setAction("static"); + updateStandStillDuration(step_time); + status_before_update_.set(*status_); } auto MiscObjectEntity::getCurrentAction() const -> std::string { - if (not npc_logic_started_) { - return "waiting"; - } else { - 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 c92aa11a2be..e9b9572f895 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -70,8 +70,8 @@ void PedestrianEntity::requestAssignRoute(const std::vector route; for (const auto & waypoint : waypoints) { if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - waypoint, status_.getBoundingBox(), true, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + waypoint, status_->getBoundingBox(), true, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { @@ -90,9 +90,6 @@ auto PedestrianEntity::requestFollowTrajectory( std::string PedestrianEntity::getCurrentAction() const { - if (!npc_logic_started_) { - return "waiting"; - } return behavior_plugin_ptr_->getCurrentAction(); } @@ -109,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 {}; @@ -139,7 +136,7 @@ void PedestrianEntity::requestWalkStraight() void PedestrianEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.laneMatchingSucceed()) { + if (status_->laneMatchingSucceed()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); @@ -149,8 +146,8 @@ void PedestrianEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & m { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), true, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + map_pose, status_->getBoundingBox(), true, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { @@ -164,13 +161,6 @@ void PedestrianEntity::cancelRequest() route_planner_.cancelRoute(); } -auto PedestrianEntity::getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & -{ - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::PEDESTRIAN; - return type; -} - auto PedestrianEntity::getEntityTypename() const -> const std::string & { static const std::string result = "PedestrianEntity"; @@ -256,31 +246,26 @@ void PedestrianEntity::setDecelerationRateLimit(double deceleration_rate) setBehaviorParameter(behavior_parameter); } -void PedestrianEntity::onUpdate(double current_time, double step_time) +auto PedestrianEntity::onUpdate(const double current_time, const double step_time) -> void { EntityBase::onUpdate(current_time, step_time); - if (npc_logic_started_) { - behavior_plugin_ptr_->setOtherEntityStatus(other_status_); - behavior_plugin_ptr_->setEntityStatus( - std::make_shared(status_)); - behavior_plugin_ptr_->setTargetSpeed(target_speed_); - behavior_plugin_ptr_->setRouteLanelets(getRouteLanelets()); - behavior_plugin_ptr_->update(current_time, step_time); - auto status_updated = behavior_plugin_ptr_->getUpdatedStatus(); - if (const auto canonicalized_lanelet_pose = status_updated->getCanonicalizedLaneletPose()) { - if (isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { - stopAtCurrentPosition(); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - return; - } + behavior_plugin_ptr_->setOtherEntityStatus(other_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); + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { + if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { + stopAtCurrentPosition(); + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + return; } - setStatus(*status_updated); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - } else { - updateEntityStatusTimestamp(current_time); } + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + EntityBase::onPostUpdate(current_time, step_time); } } // namespace entity diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index bdc885a0699..deb2a7225cf 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -59,11 +59,7 @@ void VehicleEntity::cancelRequest() auto VehicleEntity::getCurrentAction() const -> std::string { - if (not npc_logic_started_) { - return "waiting"; - } else { - return behavior_plugin_ptr_->getCurrentAction(); - } + return behavior_plugin_ptr_->getCurrentAction(); } auto VehicleEntity::getDefaultDynamicConstraints() const @@ -87,13 +83,6 @@ auto VehicleEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg: return behavior_plugin_ptr_->getBehaviorParameter(); } -auto VehicleEntity::getEntityType() const -> const traffic_simulator_msgs::msg::EntityType & -{ - static traffic_simulator_msgs::msg::EntityType type; - type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; - return type; -} - auto VehicleEntity::getEntityTypename() const -> const std::string & { static const std::string result = "VehicleEntity"; @@ -112,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 {}; @@ -121,13 +110,10 @@ auto VehicleEntity::getRouteLanelets(double horizon) -> lanelet::Ids auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray { - if (!npc_logic_started_) { - return traffic_simulator_msgs::msg::WaypointsArray(); - } try { return behavior_plugin_ptr_->getWaypoints(); } catch (const std::runtime_error & e) { - if (not status_.laneMatchingSucceed()) { + if (not status_->laneMatchingSucceed()) { THROW_SIMULATION_ERROR( "Failed to calculate waypoints in NPC logics, please check Entity : ", name, " is in a lane coordinate."); @@ -137,51 +123,48 @@ auto VehicleEntity::getWaypoints() -> const traffic_simulator_msgs::msg::Waypoin } } -void VehicleEntity::onUpdate(double current_time, double step_time) +auto VehicleEntity::onUpdate(const double current_time, const double step_time) -> void { EntityBase::onUpdate(current_time, step_time); - if (npc_logic_started_) { - behavior_plugin_ptr_->setOtherEntityStatus(other_status_); - behavior_plugin_ptr_->setEntityStatus(std::make_unique(status_)); - behavior_plugin_ptr_->setTargetSpeed(target_speed_); - auto route_lanelets = getRouteLanelets(); - behavior_plugin_ptr_->setRouteLanelets(route_lanelets); - - // recalculate spline only when input data changes - if (previous_route_lanelets_ != route_lanelets) { - previous_route_lanelets_ = route_lanelets; - try { - spline_ = std::make_shared( - hdmap_utils_ptr_->getCenterPoints(route_lanelets)); - } catch (const common::scenario_simulator_exception::SemanticError & error) { - // reset the ptr when spline cannot be calculated - spline_.reset(); - } + + behavior_plugin_ptr_->setOtherEntityStatus(other_status_); + behavior_plugin_ptr_->setCanonicalizedEntityStatus(status_); + behavior_plugin_ptr_->setTargetSpeed(target_speed_); + auto route_lanelets = getRouteLanelets(); + behavior_plugin_ptr_->setRouteLanelets(route_lanelets); + + // recalculate spline only when input data changes + if (previous_route_lanelets_ != route_lanelets) { + previous_route_lanelets_ = route_lanelets; + try { + spline_ = std::make_shared( + hdmap_utils_ptr_->getCenterPoints(route_lanelets)); + } catch (const common::scenario_simulator_exception::SemanticError & error) { + // reset the ptr when spline cannot be calculated + spline_.reset(); } - behavior_plugin_ptr_->setReferenceTrajectory(spline_); - behavior_plugin_ptr_->update(current_time, step_time); - const auto status_updated = behavior_plugin_ptr_->getUpdatedStatus(); - if (const auto canonicalized_lanelet_pose = status_updated->getCanonicalizedLaneletPose()) { - if (isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { - stopAtCurrentPosition(); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - return; - } + } + 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); + if (const auto canonicalized_lanelet_pose = status_->getCanonicalizedLaneletPose()) { + if (pose::isAtEndOfLanelets(canonicalized_lanelet_pose.value(), hdmap_utils_ptr_)) { + stopAtCurrentPosition(); + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + return; } - setStatus(*status_updated); - updateStandStillDuration(step_time); - updateTraveledDistance(step_time); - } else { - updateEntityStatusTimestamp(current_time); } + updateStandStillDuration(step_time); + updateTraveledDistance(step_time); + EntityBase::onPostUpdate(current_time, step_time); } void VehicleEntity::requestAcquirePosition(const CanonicalizedLaneletPose & lanelet_pose) { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); - if (status_.laneMatchingSucceed()) { + if (status_->laneMatchingSucceed()) { route_planner_.setWaypoints({lanelet_pose}); } behavior_plugin_ptr_->setGoalPoses({static_cast(lanelet_pose)}); @@ -191,8 +174,8 @@ void VehicleEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_ { behavior_plugin_ptr_->setRequest(behavior::Request::FOLLOW_LANE); if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, status_.getBoundingBox(), false, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + map_pose, status_->getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { @@ -219,8 +202,8 @@ void VehicleEntity::requestAssignRoute(const std::vector route; for (const auto & waypoint : waypoints) { if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - waypoint, status_.getBoundingBox(), false, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + waypoint, status_->getBoundingBox(), false, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { @@ -238,8 +221,8 @@ auto VehicleEntity::requestFollowTrajectory( std::vector waypoints; for (const auto & vertex : parameter->shape.vertices) { if ( - const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - vertex.position, status_.getBoundingBox(), false, + const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + 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/helper/helper.cpp b/simulation/traffic_simulator/src/helper/helper.cpp index db216173fdb..ccd672cdb94 100644 --- a/simulation/traffic_simulator/src/helper/helper.cpp +++ b/simulation/traffic_simulator/src/helper/helper.cpp @@ -55,7 +55,7 @@ auto constructCanonicalizedLaneletPose( const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose { if ( - auto canonicalized_lanelet_pose = canonicalize( + auto canonicalized_lanelet_pose = pose::canonicalize( traffic_simulator::helper::constructLaneletPose(lanelet_id, s, offset, roll, pitch, yaw), hdmap_utils_ptr)) { return canonicalized_lanelet_pose.value(); diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 0b87733cc37..cd8ab6d5a37 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -58,10 +58,11 @@ void TrafficController::autoSink() if (hdmap_utils_->getNextLaneletIds(lanelet_id).empty()) { LaneletPose lanelet_pose; lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = laneletLength(lanelet_id, hdmap_utils_); - const auto pose = toMapPose(lanelet_pose, hdmap_utils_); + 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 307cb7cdcf5..b6e8e8a9ff8 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -45,6 +45,16 @@ auto lateralDistance( } } +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, @@ -77,22 +87,19 @@ auto longitudinalDistance( return std::nullopt; } } else { - /// @todo here matching_distance should be passed - constexpr double matching_distance = 5.0; /** - * @brief hard coded parameter!! 5.0 is a matching distance of the toLaneletPoses function. - * A matching distance of about 1.5 lane widths is given as the matching distance to match the + * @brief A matching distance of about 1.5*lane widths is given as the matching distance to match the * Entity present on the adjacent Lanelet. + * The length of the horizontal bar must intersect with the adjacent lanelet, + * so it is always 10m regardless of the entity type. */ + constexpr double matching_distance = 5.0; + auto from_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(from), static_cast(from).lanelet_id, matching_distance, include_opposite_direction); from_poses.emplace_back(from); - /** - * @brief hard coded parameter!! 5.0 is a matching distance of the toLaneletPoses function. - * A matching distance of about 1.5 lane widths is given as the matching distance to match the - * Entity present on the adjacent Lanelet. - */ + auto to_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(to), static_cast(to).lanelet_id, matching_distance, include_opposite_direction); 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..2cd692fafff 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -5,3 +5,5 @@ add_subdirectory(src/behavior) add_subdirectory(src/job) add_subdirectory(src/simulation_clock) add_subdirectory(src/hdmap_utils) +add_subdirectory(src/utils) +add_subdirectory(src/data_type) 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/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 96bebd55008..4c0fc3d2b5f 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -77,11 +77,10 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getCurrentAction_npcNotStarted) const auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, traffic_simulator::entity_status::CanonicalizedEntityStatus( - non_canonicalized_status, hdmap_utils_ptr), + non_canonicalized_status, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659)), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); - EXPECT_FALSE(blob.isNpcLogicStarted()); - EXPECT_EQ(blob.getCurrentAction(), "waiting"); + EXPECT_EQ(blob.getCurrentAction(), "current_action_name"); } /** @@ -294,16 +293,6 @@ TEST_F(MiscObjectEntityTest_FullObject, get2DPolygon) EXPECT_POINT_EQ(polygon.at(4), ref_poly.at(4)); } -/** - * @note Test basic functionality; test whether the NPC logic is started correctly. - */ -TEST_F(MiscObjectEntityTest_FullObject, startNpcLogic) -{ - EXPECT_FALSE(misc_object.isNpcLogicStarted()); - misc_object.startNpcLogic(0.0); - EXPECT_TRUE(misc_object.isNpcLogicStarted()); -} - /** * @note Test basic functionality; test activating an out of range job with * an entity that has a positive speed and a speed range specified in the job = [0, 0] @@ -454,23 +443,9 @@ TEST_F(MiscObjectEntityTest_FullObject, requestWalkStraight) * when NPC logic is started and velocity is greater than 0. */ TEST_F(MiscObjectEntityTest_FullObject, updateStandStillDuration_startedMoving) -{ - misc_object.startNpcLogic(0.0); - misc_object.setLinearVelocity(3.0); - - EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); -} - -/** - * @note Test basic functionality; test updating stand still duration - * when NPC logic is not started. - */ -TEST_F(MiscObjectEntityTest_FullObject, updateStandStillDuration_notStarted) { misc_object.setLinearVelocity(3.0); - EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); - misc_object.setLinearVelocity(0.0); EXPECT_EQ(0.0, misc_object.updateStandStillDuration(0.1)); } @@ -482,7 +457,6 @@ TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_startedMoving) { constexpr double velocity = 3.0; constexpr double step_time = 0.1; - misc_object.startNpcLogic(0.0); misc_object.setLinearVelocity(velocity); EXPECT_EQ(1.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); @@ -491,19 +465,6 @@ TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_startedMoving) EXPECT_EQ(4.0 * step_time * velocity, misc_object.updateTraveledDistance(step_time)); } -/** - * @note Test basic functionality; test updating traveled distance correctness with NPC not started. - */ -TEST_F(MiscObjectEntityTest_FullObject, updateTraveledDistance_notStarted) -{ - constexpr double step_time = 0.1; - misc_object.setLinearVelocity(3.0); - EXPECT_EQ(0.0, misc_object.updateTraveledDistance(step_time)); - - misc_object.setLinearVelocity(0.0); - EXPECT_EQ(0.0, misc_object.updateTraveledDistance(step_time)); -} - /** * @note Test basic functionality; test stopping correctness - the goal * is to check whether the entity status is changed to stopped (no velocity etc.). @@ -528,11 +489,9 @@ TEST_F( { EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( entity_name, - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 0.0, - entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils_ptr), + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 1.0, + 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(5.0) .has_value()); @@ -548,13 +507,11 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getCanonicalizedLaneletPose_onRoadAndCro EXPECT_TRUE( traffic_simulator::entity::MiscObjectEntity( entity_name, - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils_ptr, - makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, entity_name, - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils_ptr), + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, + makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 1.0, 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(1.0) .has_value()); @@ -571,13 +528,11 @@ TEST_F( EXPECT_FALSE( traffic_simulator::entity::MiscObjectEntity( entity_name, - traffic_simulator::CanonicalizedEntityStatus( - makeEntityStatus( - hdmap_utils_ptr, - makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), - makeBoundingBox(), 0.0, entity_name, - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), - hdmap_utils_ptr), + makeCanonicalizedEntityStatus( + hdmap_utils_ptr, + makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), + makeBoundingBox(), 1.0, 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(1.0) .has_value()); 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..93512bda08d 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 @@ -109,6 +109,21 @@ class HdMapUtilsTest_CrossroadsWithStoplinesMap : public testing::Test hdmap_utils::HdMapUtils hdmap_utils; }; +class HdMapUtilsTest_KashiwanohaMap : public testing::Test +{ +protected: + HdMapUtilsTest_KashiwanohaMap() + : hdmap_utils(hdmap_utils::HdMapUtils( + 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. @@ -447,6 +462,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 +2039,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 4b68528b152..ba5f24b86c6 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -122,25 +122,31 @@ auto makeEntityStatus( auto makeCanonicalizedEntityStatus( std::shared_ptr hdmap_utils, - traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, + traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_lanelet_pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); + makeEntityStatus(hdmap_utils, canonicalized_lanelet_pose, bbox, speed, name, type), + canonicalized_lanelet_pose); } auto makeCanonicalizedEntityStatus( std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "default_entity_name", + traffic_simulator_msgs::msg::BoundingBox bbox, const double matching_distance = 1.0, + const double speed = 0.0, const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { + const auto include_crosswalk = + (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == type || + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == type); + const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, bbox, include_crosswalk, matching_distance, hdmap_utils); return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), hdmap_utils); + makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), canonicalized_lanelet_pose); } #endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ 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..6ab5ba9c6f9 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,58 @@ 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/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_light_manager.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp index fa6b3aad1ad..f5b58616f44 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light_manager.cpp @@ -18,16 +18,33 @@ #include #include -TEST(TrafficLightManager, getIds) +class TrafficLightManagerTest : public testing::Test +{ +protected: + TrafficLightManagerTest() + : manager(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0))) + { + } + traffic_simulator::TrafficLightManager manager; +}; + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +/** + * @note Test basic functionality. Test obtaining traffic light functionality + * with a non-existant laneletId. A traffic light should be created. + */ +TEST_F(TrafficLightManagerTest, getTrafficLight) { - const auto node = std::make_shared("getIds"); - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - const auto hdmap_utils_ptr = std::make_shared(path, origin); - traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); manager.getTrafficLight(34836); EXPECT_FALSE(manager.getTrafficLights().find(34836) == std::end(manager.getTrafficLights())); manager.getTrafficLight(34802); @@ -35,20 +52,15 @@ TEST(TrafficLightManager, getIds) EXPECT_EQ(manager.getTrafficLights().size(), static_cast(2)); } -TEST(TrafficLightManager, setColor) +/** + * @note Test basic functionality. Test adding a bulb to a specific traffic light with only a color specified. + */ +TEST_F(TrafficLightManagerTest, getTrafficLights_setColor) { - const auto node = std::make_shared("setColor"); - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - const auto hdmap_utils_ptr = std::make_shared(path, origin); - traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); + using Color = traffic_simulator::TrafficLight::Color; + using Status = traffic_simulator::TrafficLight::Status; + using Shape = traffic_simulator::TrafficLight::Shape; for (const auto & [id, traffic_light] : manager.getTrafficLights()) { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; manager.getTrafficLight(id).clear(); manager.getTrafficLight(id).emplace(Color::green); EXPECT_TRUE( @@ -63,20 +75,15 @@ TEST(TrafficLightManager, setColor) } } -TEST(TrafficLightManager, setArrow) +/** + * @note Test basic functionality. Test adding a bulb to a specific traffic light with all parameters specified. + */ +TEST_F(TrafficLightManagerTest, getTrafficLights_setArrow) { - const auto node = std::make_shared("setArrow"); - std::string path = - ament_index_cpp::get_package_share_directory("traffic_simulator") + "/map/lanelet2_map.osm"; - geographic_msgs::msg::GeoPoint origin; - origin.latitude = 35.61836750154; - origin.longitude = 139.78066608243; - const auto hdmap_utils_ptr = std::make_shared(path, origin); - traffic_simulator::TrafficLightManager manager(hdmap_utils_ptr); + using Color = traffic_simulator::TrafficLight::Color; + using Status = traffic_simulator::TrafficLight::Status; + using Shape = traffic_simulator::TrafficLight::Shape; for (const auto & [id, traffic_light] : manager.getTrafficLights()) { - using Color = traffic_simulator::TrafficLight::Color; - using Status = traffic_simulator::TrafficLight::Status; - using Shape = traffic_simulator::TrafficLight::Shape; manager.getTrafficLight(id).clear(); manager.getTrafficLight(id).emplace(Color::green, Status::solid_on, Shape::left); EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::left)); @@ -89,10 +96,3 @@ TEST(TrafficLightManager, setArrow) EXPECT_TRUE(manager.getTrafficLight(id).contains(Color::green, Status::solid_on, Shape::up)); } } - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); - return RUN_ALL_TESTS(); -} 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..8c96fcb2eaa --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt @@ -0,0 +1,2 @@ +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_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 250e3aad798..2a1348d11ca 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,233 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 54efdf5dfc2..14b89ebeb03 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 3.2.0 + 4.2.7 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index abca7977da9..e448bd933cf 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,253 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose 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/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index 08314d0b5a9..a33b62fb37c 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -156,14 +156,14 @@ class TestExecutor }); } - void update() + auto update() -> void { executeWithErrorHandling([this]() { - if (!api_->isEgoSpawned() && !api_->isNpcLogicStarted()) { + if (not api_->isEgoSpawned() and not api_->isNpcLogicStarted()) { api_->startNpcLogic(); } if ( - api_->isEgoSpawned() && !api_->isNpcLogicStarted() && + api_->isEgoSpawned() and not api_->isNpcLogicStarted() and api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) { api_->startNpcLogic(); } diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index ac1b8341c6c..f878889d515 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.2.0 + 4.2.7 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 4088fff42e4..04fcacd9ac4 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -129,9 +129,8 @@ class MockTrafficSimulatorAPI traffic_simulator::CanonicalizedEntityStatus getEntityStatus(const std::string & name) { getEntityStatusMock(name); - entity_status_.lanelet_pose_valid = false; - std::shared_ptr hd_map_utils_nullptr = nullptr; - return traffic_simulator::CanonicalizedEntityStatus(entity_status_, hd_map_utils_nullptr); + /// @note set invalid LaneletPose so pass std::nullopt + return traffic_simulator::CanonicalizedEntityStatus(entity_status_, std::nullopt); } void setEntityStatusNecessaryValues( diff --git a/test_runner/random_test_runner/test/test_utils.hpp b/test_runner/random_test_runner/test/test_utils.hpp index deadfd0053b..a1c69241dd8 100644 --- a/test_runner/random_test_runner/test/test_utils.hpp +++ b/test_runner/random_test_runner/test/test_utils.hpp @@ -46,8 +46,8 @@ traffic_simulator::CanonicalizedEntityStatus getCanonicalizedEntityStatus( entity_status.pose.position.x = x; entity_status.pose.position.y = y; entity_status.pose.position.z = z; - entity_status.lanelet_pose_valid = false; - return traffic_simulator::CanonicalizedEntityStatus(entity_status, hdmap_utils_ptr); + /// @note set invalid LaneletPose so pass std::nullopt + return traffic_simulator::CanonicalizedEntityStatus(entity_status, std::nullopt); } inline traffic_simulator_msgs::msg::LaneletPose makeLaneletPose( diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 9fff6401525..ba0a21255f2 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,280 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +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 + Add `EntitySelection` +* Merge branch 'master' into feature/interpreter/entity_selection +* Update workflow.txt +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge branch 'master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge branch 'feature/interpreter/entity_selection' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/refactoring_entity +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Fix typo +* Add test for LaneChangeAction +* Add test for TimeHeadwayCondition +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Add test for ReachPositionCondition +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Add test for (Relative)DistanceCondition +* Add test for StandStillCondition +* Add test for AccelerationCondition +* Add test scenario for EntitySelection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Merge remote-tracking branch 'origin/master' into feature/interpreter/entity_selection +* Contributors: Shota Minami, Tatsuya Yamasaki + 3.2.0 (2024-07-18) ------------------ * Merge remote-tracking branch 'origin/master' into fix/spawn_position_of_map_pose diff --git a/test_runner/scenario_test_runner/config/workflow.txt b/test_runner/scenario_test_runner/config/workflow.txt index 786a9c1d113..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 @@ -16,3 +18,8 @@ $(find-pkg-share scenario_test_runner)/scenario/all-in-one.yaml $(find-pkg-share scenario_test_runner)/scenario/minimal.yaml $(find-pkg-share scenario_test_runner)/scenario/prefixed-name-reference.yaml $(find-pkg-share scenario_test_runner)/scenario/stand-still.yaml +$(find-pkg-share scenario_test_runner)/scenario/EntitySelection/DistanceCondition_RelativeDistanceCondition_ReachPositionCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/EntitySelection/LaneChangeAction.yaml +$(find-pkg-share scenario_test_runner)/scenario/EntitySelection/SpeedAction_SpeedCondition_AccelerationCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/EntitySelection/TeleportAction_CollisionCondition_StandStillCondition.yaml +$(find-pkg-share scenario_test_runner)/scenario/EntitySelection/TimeHeadwayCondition.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 f981e2e2915..85de3e104c5 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.2.0 + 4.2.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/EntitySelection/DistanceCondition_RelativeDistanceCondition_ReachPositionCondition.yaml b/test_runner/scenario_test_runner/scenario/EntitySelection/DistanceCondition_RelativeDistanceCondition_ReachPositionCondition.yaml new file mode 100644 index 00000000000..1e93f23fb75 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/EntitySelection/DistanceCondition_RelativeDistanceCondition_ReachPositionCondition.yaml @@ -0,0 +1,286 @@ +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 0 + date: '2023-07-18T03:23:52.943Z' + description: '' + author: 'Shota Minami' + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + RoadNetwork: + LogicFile: { filepath: "$(find-pkg-share kashiwanoha_map)/map/lanelet2_map.osm" } + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: npc0 + Vehicle: &default-vehicle + name: default-vehicle + vehicleCategory: car + BoundingBox: + Center: { x: 0, y: 0, z: 0 } + Dimensions: { width: 1.8, length: 4, height: 2.5 } + Performance: { maxSpeed: 30, maxAcceleration: INF, maxDeceleration: INF } + Axles: + FrontAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: 1, positionZ: 0.3 } + RearAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: -1, positionZ: 0.3 } + Properties: + Property: [] + - name: npc1 + Vehicle: + <<: *default-vehicle + - name: npc2 + Vehicle: + <<: *default-vehicle + EntitySelection: + - name: npc_0_1 + Members: + EntityRef: [ entityRef: npc0, entityRef: npc1 ] + - name: npc_0_2 + Members: + EntityRef: [ entityRef: npc0, entityRef: npc2 ] + Storyboard: + Init: + Actions: + Private: + - entityRef: npc0 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 0 + offset: 0 + Orientation: { type: relative, h: 0, p: 0, r: 0 } + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + - entityRef: npc1 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 10 + offset: 0 + Orientation: { type: relative, h: 0, p: 0, r: 0 } + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + - entityRef: npc2 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 20 + offset: 0 + Orientation: { type: relative, h: 0, p: 0, r: 0 } + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - name: '' + maximumExecutionCount: 1 + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: 'distance_test' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(DistanceCondition Satisfied)' + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: [ entityRef: npc_0_1 ] + EntityCondition: + DistanceCondition: + freespace: False + relativeDistanceType: longitudinal + rule: greaterThan + value: 5 + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 20 + offset: 0 + Orientation: { type: relative, h: 0, p: 0, r: 0 } + - name: 'reach_position_test' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(ReachPositionCondition Satisfied)' + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: [ entityRef: npc_0_2 ] + EntityCondition: + ReachPositionCondition: + tolerance: 15 + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 10 + offset: 0 + Orientation: { type: relative, h: 0, p: 0, r: 0 } + - name: 'relative_distance_test' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(RelativeDistanceCondition Satisfied)' + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: [ entityRef: npc_0_1 ] + EntityCondition: + RelativeDistanceCondition: + entityRef: npc2 + freespace: False + relativeDistanceType: longitudinal + rule: greaterThan + value: 5 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: '' + Act: + - name: _EndCondition + ManeuverGroup: + - name: '' + maximumExecutionCount: 1 + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: distance_test + storyboardElementType: action + state: completeState + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: reach_position_test + storyboardElementType: action + state: completeState + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: relative_distance_test + storyboardElementType: action + state: completeState + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 5 + rule: greaterThan + 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/EntitySelection/LaneChangeAction.yaml b/test_runner/scenario_test_runner/scenario/EntitySelection/LaneChangeAction.yaml new file mode 100644 index 00000000000..4c4e061edb4 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/EntitySelection/LaneChangeAction.yaml @@ -0,0 +1,215 @@ +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 0 + date: '2021-09-06T09:09:41.856Z' + description: '' + author: '' + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + CatalogLocation: [] + RoadNetwork: + LogicFile: + filepath: $(find-pkg-share kashiwanoha_map)/map + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: ego + Vehicle: &vehicle + name: '' + vehicleCategory: car + BoundingBox: + Center: { x: 0, y: 0, z: 0 } + Dimensions: { width: 2.25, length: 4.77, height: 2.5 } + Performance: + maxSpeed: INF + maxAcceleration: INF + maxDeceleration: INF + Axles: + FrontAxle: + maxSteering: 3.1415 + wheelDiameter: 0.6 + trackWidth: 4 + positionX: 1 + positionZ: 0.3 + RearAxle: + maxSteering: 3.1415 + wheelDiameter: 0.6 + trackWidth: 4 + positionX: -1 + positionZ: 0.3 + Properties: + Property: [] + - name: npc + Vehicle: + <<: *vehicle + EntitySelection: + - name: vehicles + Members: + ByType: [ objectType: vehicle ] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 10 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 5 + - entityRef: npc + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 0 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 5 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: [ entityRef: vehicles ] + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 1 + rule: greaterThan + Action: + - name: lane_change + PrivateAction: + LateralAction: + LaneChangeAction: + LaneChangeActionDynamics: + dynamicsDimension: time + dynamicsShape: cubic + value: 3 + LaneChangeTarget: + RelativeTargetLane: + entityRef: npc + value: -1 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: '' + Act: + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 20 + rule: greaterThan + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: lane_change + storyboardElementType: action + state: completeState + 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/EntitySelection/SpeedAction_SpeedCondition_AccelerationCondition.yaml b/test_runner/scenario_test_runner/scenario/EntitySelection/SpeedAction_SpeedCondition_AccelerationCondition.yaml new file mode 100644 index 00000000000..a54d0d20766 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/EntitySelection/SpeedAction_SpeedCondition_AccelerationCondition.yaml @@ -0,0 +1,267 @@ +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 0 + date: '2023-07-18T15:35:37.294Z' + description: '' + author: Shota Minami + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + CatalogLocation: [] + RoadNetwork: + LogicFile: + filepath: $(find-pkg-share kashiwanoha_map)/map + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: npc0 + Vehicle: &default-vehicle + name: '' + vehicleCategory: car + BoundingBox: + Center: { x: 0, y: 0, z: 0 } + Dimensions: { width: 2.25, length: 4.77, height: 2.5 } + Performance: { maxSpeed: INF, maxAcceleration: INF, maxDeceleration: INF } + Axles: + FrontAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: 1, positionZ: 0.3 } + RearAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: -1, positionZ: 0.3 } + Properties: + Property: [] + - name: npc1 + Vehicle: + <<: *default-vehicle + - name: npc2 + Vehicle: + <<: *default-vehicle + EntitySelection: + - name: npcs_ref + Members: + EntityRef: [ entityRef: npc1, entityRef: npc2 ] + - name: npcs_by_type + Members: + ByType: [ objectType: vehicle ] + Storyboard: + Init: + Actions: + Private: + - entityRef: npc0 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 20 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + - entityRef: npc1 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 10 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + - entityRef: npc2 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 0 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: [ entityRef: npcs_ref, entityRef: npc0 ] + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + Action: + - name: speedup + PrivateAction: + LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: rate + value: 10 + dynamicsShape: linear + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 50 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: '' + Act: + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: 'Acceleration' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: npcs_by_type + EntityCondition: + AccelerationCondition: + rule: greaterThan + value: 9 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(AccelerationCondition Satisfied)' + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: 'Speed' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: npcs_by_type + EntityCondition: + SpeedCondition: + rule: equalTo + value: 50 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(SpeedCondition Satisfied)' + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: speedup + storyboardElementType: action + state: completeState + 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/EntitySelection/TeleportAction_CollisionCondition_StandStillCondition.yaml b/test_runner/scenario_test_runner/scenario/EntitySelection/TeleportAction_CollisionCondition_StandStillCondition.yaml new file mode 100644 index 00000000000..b30c9490599 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/EntitySelection/TeleportAction_CollisionCondition_StandStillCondition.yaml @@ -0,0 +1,229 @@ +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 0 + date: '2023-07-18T03:23:52.943Z' + description: '' + author: 'Shota Minami' + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + RoadNetwork: + LogicFile: { filepath: "$(find-pkg-share kashiwanoha_map)/map/lanelet2_map.osm" } + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: npc0 + Vehicle: &default-vehicle + name: default-vehicle + vehicleCategory: car + BoundingBox: + Center: { x: 0, y: 0, z: 0 } + Dimensions: { width: 1.8, length: 4, height: 2.5 } + Performance: { maxSpeed: 30, maxAcceleration: INF, maxDeceleration: INF } + Axles: + FrontAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: 1, positionZ: 0.3 } + RearAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: -1, positionZ: 0.3 } + Properties: + Property: [] + - name: npc1 + Vehicle: + <<: *default-vehicle + - name: npc2 + Vehicle: + <<: *default-vehicle + EntitySelection: + - name: npcs + Members: + EntityRef: [ entityRef: npc1, entityRef: npc2 ] + Storyboard: + Init: + Actions: + Private: + - entityRef: npc0 + PrivateAction: + - TeleportAction: + Position: + LanePosition: &collision-position + roadId: '' + laneId: '34513' + s: 20 + offset: 0 + Orientation: { type: relative, h: 0, p: 0, r: 0 } + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + - entityRef: npcs + PrivateAction: + - TeleportAction: + Position: + LanePosition: + <<: *collision-position + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - name: '' + maximumExecutionCount: 1 + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: 'collision_test' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(CollisionCondition1 Satisfied)' + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: npcs ] + EntityCondition: + CollisionCondition: + EntityRef: { entityRef: npc0 } + - name: '' + priority: parallel + Action: + - name: collision_test_1 + UserDefinedAction: + CustomCommandAction: + type: 'test(CollisionCondition1 Satisfied)' + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: [ entityRef: npc1 ] + EntityCondition: + CollisionCondition: + EntityRef: { entityRef: npc2 } + - name: 'stand_still_test' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(StandStillCondition Satisfied)' + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: [ entityRef: npcs, entityRef: npc0 ] + EntityCondition: + StandStillCondition: + duration: 5 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: '' + Act: + - name: _EndCondition + ManeuverGroup: + - name: '' + maximumExecutionCount: 1 + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: collision_test + storyboardElementType: action + state: completeState + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: stand_still_test + storyboardElementType: action + state: completeState + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 5 + rule: greaterThan + 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/EntitySelection/TimeHeadwayCondition.yaml b/test_runner/scenario_test_runner/scenario/EntitySelection/TimeHeadwayCondition.yaml new file mode 100644 index 00000000000..fc2671a2175 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/EntitySelection/TimeHeadwayCondition.yaml @@ -0,0 +1,220 @@ +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 0 + date: '2023-07-25T14:59:14.901Z' + description: '' + author: Shota Minami + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + CatalogLocation: [] + RoadNetwork: + LogicFile: + filepath: $(find-pkg-share kashiwanoha_map)/map + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: npc0 + Vehicle: &default-vehicle + name: '' + vehicleCategory: car + BoundingBox: + Center: { x: 0, y: 0, z: 0 } + Dimensions: { width: 2.25, length: 4.77, height: 2.5 } + Performance: { maxSpeed: INF, maxAcceleration: INF, maxDeceleration: INF } + Axles: + FrontAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: 1, positionZ: 0.3 } + RearAxle: { maxSteering: 3.1415, wheelDiameter: 0.6, trackWidth: 4, positionX: -1, positionZ: 0.3 } + Properties: + Property: [] + - name: npc1 + Vehicle: + <<: *default-vehicle + - name: npc2 + Vehicle: + <<: *default-vehicle + EntitySelection: + - name: npcs + Members: + EntityRef: [ entityRef: npc0, entityRef: npc1 ] + Storyboard: + Init: + Actions: + Private: + - entityRef: npc0 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 0 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 10 + - entityRef: npc1 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + 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: 10 + - entityRef: npc2 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 100 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: 'time_headway_test' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: 'test(TimeHeadwayCondition Satisfied)' + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: all + EntityRef: [ entityRef: npcs ] + EntityCondition: + TimeHeadwayCondition: + alongRoute: true + entityRef: npc2 + freespace: false + rule: lessThan + value: 10 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: '' + Act: + - name: _EndCondition + ManeuverGroup: + - name: '' + maximumExecutionCount: 1 + Actors: + selectTriggeringEntities: false + EntityRef: [] + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: time_headway_test + storyboardElementType: action + state: completeState + - name: '' + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 5 + rule: greaterThan + 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/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: []