diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 880cdaf86e4..83c3d2d4ffd 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -24,8 +24,6 @@ A clear and concise description of what you expected to happen. If applicable, add screenshots to help explain your problem. **Desktop (please complete the following information):** - - OS: [e.g. iOS] - - Browser [e.g. chrome, safari] - - Version [e.g. 22] + - Ubuntu Version [e.g. 22.04] - ROS 2 version - DDS diff --git a/.github/ISSUE_TEMPLATE/release.md b/.github/ISSUE_TEMPLATE/release.md deleted file mode 100644 index 8afe53b63d0..00000000000 --- a/.github/ISSUE_TEMPLATE/release.md +++ /dev/null @@ -1,13 +0,0 @@ ---- -name: Release -about: Describe this issue template's purpose here. -title: Release [Version] -labels: release -assignees: hakuturu583 - ---- - -Please check below before sending this issue. - -- [ ] Did you update [ReleaseNotes.md?](https://github.com/tier4/scenario_simulator_v2/blob/master/docs/ReleaseNotes.md) -- [ ] Issue title is valid? (example : Release 0.2.0) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 288303a67e9..fa39f7d2d40 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -1,13 +1,36 @@ -## Types of PR +# Description -- [ ] New Features -- [ ] Upgrade of existing features -- [ ] Bugfix +> Please check examples and comment out this sentence. Minimal example is [here](pull_request_samples/example_simple.md) and detailed example is [here](pull_request_samples/example_detail.md) -## Link to the issue +## Abstract -## Description +> [Required] This section is required, keep it short, clear and to the point. +> Delete this sentence and explain this pull request shortly. -## How to review this PR. +## Background -## Others +> [Optional] If there is no background information that needs explanation (e.g., just a typo correction, etc.), you can skip this section. +> Delete this sentence and explain the circumstances that led to this pull request being sent. + +## Details + +> [Optional] If there are only differences whose effects are so obvious that no explanation is needed, or if there are no differences in the code (e.g., documentation additions), you can skip this section. +> Delete this sentence and describe this pull request. +> For example, it is desirable to describe the specifications of added functions, and detailed explanations of bugs that have been fixed. + +## References + +> [Optional] If the referenced material does not exist, you can skip this section. +> Describe any standards, algorithms, books, articles, etc. that you referenced when creating the pull request. If there is any novelty, mention it. + +# Destructive Changes + +> [Optional] If no destructive change exists, you can skip this section. +> Include a description of the changes and a migration guide and send the pull request with a bump major label. (Example : https://github.com/tier4/scenario_simulator_v2/pull/1139) +> Otherwise, skip the "Destructive Changes" section and make sure this pull request is labeled `bump minor` or `bump patch`. + +# Known Limitations + +> [Optional] If there are no known limitations that you can think of, you can skip this section. If there are any limitations on the features or fixes added by this pull request, delete this sentence and clearly describe them. +> For example, the lane matching algorithm currently (1/25/2024) employed is unable to match Entity that is heavily tilted with respect to the lane, and it is difficult to throw an exception. +> If the developer is aware of the existence of such problems or limitations, describe them in detail. The problems or limitation should be listed as an Issue on GitHub and a link to it should be provided in this section. diff --git a/.github/pull_request_samples/example_detail.md b/.github/pull_request_samples/example_detail.md new file mode 100644 index 00000000000..c31fe9672cd --- /dev/null +++ b/.github/pull_request_samples/example_detail.md @@ -0,0 +1,85 @@ +# Description + +## Abstract + +Fixed bags below. + +- Removed equal operators for geometry_msgs::msg::Point and geometry_msgs::msg::Vector3, because they were ambiguous. +- Fixed the bug which caused the intersection functions using vector to look past the last element of the vector and return wrong results. +- Fixed a bug where the LineSegment class could be constructed with a geometry_msgs::msg::Vector3 of size = 0. This lead to initialization of end_point with nan values. +- Fixed the getMinValue and getMaxValue bug where when an empty vector was passed the function tried to de-reference vector.end() and the whole program crashed. +- Fixed a getPolygon bug which caused the function to generate incorrect number of points. +- Added support for negative curvature values which were already supported by HermiteCurve. This incompatibility lead to errors. +- Fixed spline collision implementation error which caused spline to add normalized length to absolute lengths which is incorrect. +- Fixed CatmullRomSubspline collision detection by enabling the HermiteCurve and CatmullRomSpline to have multiple collisions detected and then choosing in the subspline the collisions that occur inside the subspline. + +## Background + +Fixed several residual problems in the geometric calculation library that were causing incorrect scenario execution results. + +## Details + +This PR fixes how the length of the curve is computed + +| Before fix | After fix | +| --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Lengths of the first two Curves (A and B) are calculated correctly (because these are full Curve lengths), but the distance in the third Curve (C) is calculated as half of the normalized length of the Curve (0.5). This value is added and the result and distance to the collision in the spline is equal to 20.5 | Lengths of the first two Curves (A and B) are calculated correctly (because these are full Curve lengths) and the distance in the third Curve (C) is also calculated correctly because the collision distance is being denormalized (multiplied by the full length of the Curve) which is equal to 5 (0.5 * 10).This value is added and the result and distance to the collision in the spline is equal to 25 | +| ![RJD-753_base](https://github.com/tier4/scenario_simulator_v2/assets/87643052/18b0f1a5-5370-4cf3-a60a-c1af05448d50) | ![RJD-753_eval](https://github.com/tier4/scenario_simulator_v2/assets/87643052/87570089-bf77-4be8-b950-e3f1fb8499a9) | + +## References + +- [determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed](https://gamedev.stackexchange.com/questions/14985/determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed) + - This link is an example and is not directly related to this sample. + +# Destructive Changes + +| traffic_simulator/OpenSCENARIO features | Severity | Fix | +|----------------------------------------------------------|-------------|--------------| +| FollowFrontEntityAction | low - distance kept to the front entity might be a bit larger which might influence scenarios which are relying on specific behavior in this action but it does not seem to be a lot of scenarios | Conditions which check this distance should be increased accordingly - trial and error seem to be the best way to find how much change the condition without changing the essence of the scenario| +| Computing the distance to stop_line or other "obstacles" | low - it will increase this distance making it a bit safer but if the scenario conditions rely on this value it might cause issues. No scenario, however, was noticed to be influenced by that to the extent that caused them to fail | Conditions that check this distance should be increased accordingly - trial and error seem to be the best way to find how much change the condition without changing the essence of the scenario | +| Distance-based lane change | Medium - regression is known which was caused by the fix. Cut-in scenarios which are written in a rather strict way might be influenced by this change | Decreasing the distance on which the lane change should occur or change the speed of the vehicles taking part in the scenario - example in the section below | + +This scenario usually passes without the fix but always fails after the fix is added. The scenario is based on [shinjuku_map](https://github.com/tier4/AWSIM/releases/download/v1.2.0/shinjuku_map.zip). + +[scenario.yml.txt](https://github.com/tier4/scenario_simulator_v2/files/13707779/scenario.yml.txt) + +An issue in this scenario is lane change action: + +``` +LaneChangeAction: + LaneChangeActionDynamics: + dynamicsDimension: distance + value: 10 + dynamicsShape: cubic + LaneChangeTarget: + AbsoluteTargetLane: + value: '37' +``` + +To fix it: +- The speed of Ego vehicle can be decreased slightly +- The speed of NPC motorcycle can be increased slightly +- The lane change distance can be decreased slightly + +The exact amount of the change of the values above is hard to estimate because it is very dependent on the specific scenario - like the vehicle position within the lanelet might influence how much the normalized lanelet length was different than not normalized length. + +Here is the scenario that uses the third possible fix - decreasing the distance on which the lane change action takes place. One meter decrease makes the scenario pass again. + +``` +LaneChangeAction: + LaneChangeActionDynamics: + dynamicsDimension: distance + value: 9 + dynamicsShape: cubic + LaneChangeTarget: + AbsoluteTargetLane: + value: '37' +``` + +Full fixed scenario +[scenario.yml.txt](https://github.com/tier4/scenario_simulator_v2/files/13707839/scenario.yml.txt) + +# Known Limitations + +- If the curvature is very large, the calculation may fail. + - This link is an example and is not directly related to this sample. diff --git a/.github/pull_request_samples/example_simple.md b/.github/pull_request_samples/example_simple.md new file mode 100644 index 00000000000..2fc5f7d0f1d --- /dev/null +++ b/.github/pull_request_samples/example_simple.md @@ -0,0 +1,5 @@ +# Description + +## Abstract + +Corrected "Vehicle" incorrectly spelled as "Vehicle". diff --git a/.github/workflows/BuildAndRun.yaml b/.github/workflows/BuildAndRun.yaml index 72cc378c8a5..ededd8635fa 100644 --- a/.github/workflows/BuildAndRun.yaml +++ b/.github/workflows/BuildAndRun.yaml @@ -1,4 +1,4 @@ -name: BuildAndRun +name: Build and run on: workflow_dispatch: @@ -19,7 +19,7 @@ on: - master jobs: job1: - name: BuildAndRun + name: Build and run runs-on: ubuntu-22.04 timeout-minutes: 180 container: ros:${{ matrix.rosdistro }} @@ -30,7 +30,7 @@ jobs: matrix: rosdistro: [humble] steps: - - name: suppress warnings + - name: Suppress warnings run: | git config --global --add safe.directory '*' @@ -39,10 +39,12 @@ jobs: fetch-depth: 0 token: ${{ secrets.GITHUB_TOKEN }} + - run: git clone https://github.com/RobotecAI/scenario_simulator_v2_scenarios.git + - name: Search packages in this repository id: list_packages run: | - echo package_list=$(colcon list --names-only | sed -e ':loop; N; $!b loop; s/\n/ /g') >> $GITHUB_OUTPUT + echo package_list=$(colcon list --names-only | tr '\n' ' ') >> $GITHUB_OUTPUT - name: Show target packages run: | @@ -62,7 +64,7 @@ jobs: - name: Resolve rosdep run: | cd ~/ros2_ws - apt-get update + apt update apt install -y python3-pip rosdep update --include-eol-distros rosdep install -iy --from-paths src --rosdistro ${{ matrix.rosdistro }} @@ -95,8 +97,15 @@ jobs: - name: Scenario test run: | source ~/ros2_ws/install/setup.bash - cd ~/ros2_ws - source install/local_setup.bash + source ~/ros2_ws/install/local_setup.bash ros2 launch scenario_test_runner scenario_test_runner.launch.py workflow:='$(find-pkg-share scenario_test_runner)/config/workflow_example.yaml' global_frame_rate:=20 ros2 run scenario_test_runner result_checker.py /tmp/scenario_test_runner/result.junit.xml shell: bash + + - name: Basic test + run: | + source ~/ros2_ws/install/setup.bash + source ~/ros2_ws/install/local_setup.bash + ros2 launch scenario_test_runner scenario_test_runner.launch.py workflow:='$(find-pkg-share scenario_simulator_v2_scenarios)/workflow/basic.yaml' + ros2 run scenario_test_runner result_checker.py /tmp/scenario_test_runner/result.junit.xml + shell: bash diff --git a/.github/workflows/CheckAndCommentAction.yaml b/.github/workflows/CheckAndCommentAction.yaml deleted file mode 100644 index c86fd13228c..00000000000 --- a/.github/workflows/CheckAndCommentAction.yaml +++ /dev/null @@ -1,44 +0,0 @@ -name: CheckAndComment -on: - pull_request: - -jobs: - CheckFileChanged: - name: Check File Changed - runs-on: ubuntu-20.04 - outputs: - files_changed: ${{ steps.file_changes.outputs.files }} - steps: - - uses: actions/checkout@v2 - - id: file_changes - uses: trilom/file-changes-action@v1.2.3 - CheckBugPullRequest: - if: ${{ github.event.label.name == 'bug' }} - name: Check Bug Pull Request - runs-on: ubuntu-20.04 - needs: CheckFileChanged - steps: - - name: Check Release Note - if: contains(fromJson(needs.CheckFileChanged.outputs.files_changed), 'docs/ReleaseNotes.md') == false - uses: mshick/add-pr-comment@v1 - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - with: - message: | - You labeld this PR as "bug", but Release Note does not updated, please check your PR. - allow-repeats: true - CheckFeaturePullRequest: - if: ${{ github.event.label.name == 'feature' }} - name: Check Feature Pull Request - runs-on: ubuntu-20.04 - needs: CheckFileChanged - steps: - - name: Check Release Note - if: contains(fromJson(needs.CheckFileChanged.outputs.files_changed), 'docs/ReleaseNotes.md') == false - uses: mshick/add-pr-comment@v1 - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - with: - message: | - You labeld this PR as "feature", but Release Note does not updated, please check your PR. - allow-repeats: true diff --git a/.github/workflows/CheckBranchUpToDate.yaml b/.github/workflows/CheckBranchUpToDate.yaml new file mode 100644 index 00000000000..a161bf643d9 --- /dev/null +++ b/.github/workflows/CheckBranchUpToDate.yaml @@ -0,0 +1,29 @@ +name: Check branch up to date +on: + pull_request: + branches: + - master + merge_group: + types: [checks_requested] + +jobs: + check_branch_up_to_date: + name: Check branch up to date + timeout-minutes: 10 + runs-on: ubuntu-22.04 + steps: + - name: checkout + uses: actions/checkout@v2 + with: + fetch-depth: 0 + fetch-tags: true + - name: Check if branches are up to date + shell: bash + run: | + BASE=$(git merge-base origin/master HEAD) + if [ $BASE = $(git rev-parse origin/master) ]; then + echo "Branches are up to date, skipping merge." + else + echo "Branches are not up to date, merge is required." + exit 1 + fi diff --git a/.github/workflows/CheckLabel.yaml b/.github/workflows/CheckLabel.yaml new file mode 100644 index 00000000000..b2aa1ee3ef9 --- /dev/null +++ b/.github/workflows/CheckLabel.yaml @@ -0,0 +1,23 @@ +name: Check label +on: + pull_request: + branches: + - master + label: + types: + - created + - edited + - labeled + - unlabeled + +jobs: + check_label: + name: Check label + runs-on: ubuntu-22.04 + timeout-minutes: 10 + steps: + - name: Check required label + if: (!contains(github.event.pull_request.labels.*.name, 'bump patch')) && + (!contains(github.event.pull_request.labels.*.name, 'bump minor')) && + (!contains(github.event.pull_request.labels.*.name, 'bump major')) + run: exit 1 diff --git a/.github/workflows/Docker.yaml b/.github/workflows/Docker.yaml index f476a722b9c..f279065ca53 100644 --- a/.github/workflows/Docker.yaml +++ b/.github/workflows/Docker.yaml @@ -19,7 +19,7 @@ on: types: [published] jobs: job1: - name: Build Docker Image + name: Build Docker image runs-on: ubuntu-20.04 timeout-minutes: 180 strategy: @@ -28,7 +28,7 @@ jobs: steps: - uses: actions/checkout@v3 - - name: Setup Docker Buildx + - name: Setup Docker buildx uses: docker/setup-buildx-action@v2 - name: Login to GitHub Container Registry diff --git a/.github/workflows/Documentation.yaml b/.github/workflows/Documentation.yaml index 2eca581e53d..627f83143bd 100644 --- a/.github/workflows/Documentation.yaml +++ b/.github/workflows/Documentation.yaml @@ -13,7 +13,7 @@ on: jobs: generate: - name: Generate Documentation + name: Generate documentation runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v2-beta diff --git a/.github/workflows/DocumentationLinkCheck.yaml b/.github/workflows/DocumentationLinkCheck.yaml index d3b4a11600c..9f3479424a8 100644 --- a/.github/workflows/DocumentationLinkCheck.yaml +++ b/.github/workflows/DocumentationLinkCheck.yaml @@ -1,4 +1,4 @@ -name: DocumentationLinkCheck +name: Documentation link check on: repository_dispatch: @@ -13,7 +13,7 @@ jobs: steps: - uses: actions/checkout@v3 - - name: Link Checker + - name: Check documentation link id: lychee uses: lycheeverse/lychee-action@v1.5.0 with: diff --git a/.github/workflows/InterfaceUpdateNotification.yaml b/.github/workflows/InterfaceUpdateNotification.yaml index f7b6fb746d7..fcacbeb93dc 100644 --- a/.github/workflows/InterfaceUpdateNotification.yaml +++ b/.github/workflows/InterfaceUpdateNotification.yaml @@ -1,4 +1,4 @@ -name: InterfaceUpdateNotification +name: Interface update notification on: pull_request: branches: diff --git a/.github/workflows/LineLint.yaml b/.github/workflows/LineLint.yaml index 31a0b107822..e2b4c4b8187 100644 --- a/.github/workflows/LineLint.yaml +++ b/.github/workflows/LineLint.yaml @@ -1,4 +1,4 @@ -name: LineLint +name: Line lint on: workflow_dispatch: push: diff --git a/.github/workflows/PostCheckList.yaml b/.github/workflows/PostCheckList.yaml new file mode 100644 index 00000000000..052ce29d8cd --- /dev/null +++ b/.github/workflows/PostCheckList.yaml @@ -0,0 +1,36 @@ +name: Post check list +on: + pull_request: + branches: + - master + types: [opened, reopened] + +jobs: + post_check_list: + name: Post check list + timeout-minutes: 10 + runs-on: ubuntu-22.04 + permissions: + actions: write + checks: write + contents: write + deployments: write + issues: write + packages: write + pull-requests: write + repository-projects: write + security-events: write + statuses: write + steps: + - uses: alawiii521/current-pr-comment@v1.0 + with: + comment: | + # Checklist for reviewers ☑️ + All references to "You" in the following text refer to the code reviewer. + - [ ] Is this pull request written in a way that is easy to read from a third-party perspective? + - [ ] Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request? + - [ ] If this pull request contains a destructive change, does this pull request contain the migration guide? + - [ ] Labels of this pull request are valid? + - [ ] All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line. + - [ ] The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line. + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/Release.yaml b/.github/workflows/Release.yaml index 20380450d94..f0328615ccb 100644 --- a/.github/workflows/Release.yaml +++ b/.github/workflows/Release.yaml @@ -1,32 +1,121 @@ -# This file is automatically deployed from https://github.com/at-wat/.rospkg-assets. -# Please don't directly edit; update at-wat/.rospkg-assets instead. - name: Release on: - issues: - types: [opened, edited] - + pull_request: + branches: + - master + types: + - closed + - created + - edited + - labeled + - opened + - reopened + - synchronize + - unlabeled +concurrency: release + jobs: - Release: - runs-on: ubuntu-latest - if: startsWith(github.event.issue.title, 'Release ') + release: + name: Release + runs-on: ubuntu-22.04 + permissions: + actions: write + checks: write + contents: write + deployments: write + issues: write + packages: write + pull-requests: write + repository-projects: write + security-events: write + statuses: write + timeout-minutes: 10 + container: ros:humble steps: - - name: checkout + - name: Restore branch + uses: levonet/action-restore-branch@master + - name: Install bloom + run: apt update && apt install -y python3-bloom git + - name: Checkout uses: actions/checkout@v2 - - name: create release - id: create_release - uses: at-wat/catkin-release-action@v1 with: - issue_title: ${{ github.event.issue.title }} - git_user: Masaya Kataoka - git_email: ms.kataoka@gmail.com - github_token: ${{ secrets.GITHUB_TOKEN }} - - name: open pull-request - uses: repo-sync/pull-request@v2 + ref: master + fetch-depth: 0 + fetch-tags: true + - name: Setup git + run: | + git config --global user.name "Release Bot" + git config --global user.email "action@github.com" + git config --global --add safe.directory /__w/scenario_simulator_v2/scenario_simulator_v2 + git config --global credential.helper ${{ secrets.GITHUB_TOKEN }} + git config pull.rebase false + - name: Get old version + id: old_version + run: | + echo "old_version=$(catkin_package_version)" >> $GITHUB_OUTPUT + - name: Merge branch + run: git pull origin ${{ github.head_ref }} + - name: Bump patch version + if: (contains(github.event.pull_request.labels.*.name, 'bump patch')) + run: | + catkin_generate_changelog -y + catkin_tag_changelog --bump patch + catkin_package_version --bump patch + - name: Bump minor version + if: (contains(github.event.pull_request.labels.*.name, 'bump minor')) + run: | + catkin_generate_changelog -y + catkin_tag_changelog --bump minor + catkin_package_version --bump minor + - name: Bump major version + if: (contains(github.event.pull_request.labels.*.name, 'bump major')) + run: | + catkin_generate_changelog -y + catkin_tag_changelog --bump major + catkin_package_version --bump major + - name: Get new version + id: new_version + run: | + echo "new_version=$(catkin_package_version)" >> $GITHUB_OUTPUT + - name: Commit changes + if: github.event.pull_request.merged == true + run: | + git add . + git commit -m "Bump version of scenario_simulator_v2 from version ${{ steps.old_version.outputs.old_version }} to version ${{ steps.new_version.outputs.new_version }}" + - name: Pushing to the protected branch 'master' + if: github.event.pull_request.merged == true + uses: CasperWA/push-protected@v2 with: - source_branch: ${{ steps.create_release.outputs.created_branch }} - destination_branch: master - pr_title: Release ${{ steps.create_release.outputs.version}} - pr_body: close \#${{ github.event.issue.number }} - pr_assignee: ${{ github.actor }} - github_token: ${{ secrets.BLOOM_GITHUB_TOKEN }} + token: ${{ secrets.BLOOM_GITHUB_TOKEN }} + branch: master + force: true + - name: Get linked issues + id: linked_issues + uses: hossainemruz/linked-issues@main + with: + pr_url: ${{github.event.pull_request.html_url}} + format: ExternalIssueRef + continue-on-error: true + - name: Update release description + run: | + echo "${{ github.event.pull_request.body }}" >> release_description.txt + echo "" >> release_description.txt + echo "# Related Issues " >> release_description.txt + echo "${{ steps.linked_issues.outputs.issues }}" >> release_description.txt + - name: Get release description + id: get_release_description + uses: mathiasvr/command-output@v2.0.0 + with: + run: cat release_description.txt + - name: Create a GitHub release + if: github.event.pull_request.merged == true + uses: ncipollo/release-action@v1 + with: + tag: ${{ steps.new_version.outputs.new_version }} + name: ${{ steps.new_version.outputs.new_version }} + body: ${{ steps.get_release_description.outputs.stdout }} + - name: Delete branch + if: github.event.pull_request.merged == true + uses: SvanBoxel/delete-merged-branch@main + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} diff --git a/.github/workflows/SimModelUpdateNotification.yaml b/.github/workflows/SimModelUpdateNotification.yaml index 4467d60703e..49dc7a0a4c4 100644 --- a/.github/workflows/SimModelUpdateNotification.yaml +++ b/.github/workflows/SimModelUpdateNotification.yaml @@ -1,4 +1,4 @@ -name: SimModelUpdateNotification +name: Simulation Model Update Notification on: pull_request: paths: diff --git a/.github/workflows/SpellCheck.yaml b/.github/workflows/SpellCheck.yaml index 9dcd8b91b58..612b3e54d61 100644 --- a/.github/workflows/SpellCheck.yaml +++ b/.github/workflows/SpellCheck.yaml @@ -1,4 +1,4 @@ -name: SpellCheck +name: Spell check on: workflow_dispatch: diff --git a/.github/workflows/SpellCheckAll.yaml b/.github/workflows/SpellCheckAll.yaml index 361d474bf44..c3c5633773b 100644 --- a/.github/workflows/SpellCheckAll.yaml +++ b/.github/workflows/SpellCheckAll.yaml @@ -1,4 +1,4 @@ -name: SpellCheckAll +name: Spell check all on: workflow_dispatch: diff --git a/.github/workflows/SyncReleaseNote.yaml b/.github/workflows/SyncReleaseNote.yaml deleted file mode 100644 index 449bff61abd..00000000000 --- a/.github/workflows/SyncReleaseNote.yaml +++ /dev/null @@ -1,19 +0,0 @@ -name: SyncReleaseNote -on: - workflow_dispatch: - push: - branches: - - master - -jobs: - dev: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - uses: cupcakearmy/confluence-markdown-sync@v1.0.1 - with: - from: './docs/ReleaseNotes.md' - to: '2508390892' - cloud: tier4 - user: kotaro.yoshimoto@tier4.jp - token: ${{ secrets.CONFLUENCE_TOKEN }} diff --git a/.github/workflows/ToolDispatch.yaml b/.github/workflows/ToolDispatch.yaml index 2f7e8f67571..5f40a6f9dfd 100644 --- a/.github/workflows/ToolDispatch.yaml +++ b/.github/workflows/ToolDispatch.yaml @@ -1,4 +1,4 @@ -name: ToolDispatch +name: Tool dispatch on: pull_request: types: [labeled] diff --git a/.github/workflows/custom_spell.json b/.github/workflows/custom_spell.json index 205b0bb1497..a3e2ffebac7 100644 --- a/.github/workflows/custom_spell.json +++ b/.github/workflows/custom_spell.json @@ -6,32 +6,33 @@ "Canonicalized", "canonicalizing", "classname", + "classname", + "Dawid", "DBOOST", "DBUILD", - "DWITH_INTEGRATION_TEST", - "Kataoka", - "Masaya", - "Mersenne", - "Monic", - "NSPACES", - "Petrich", - "PREDEF", - "TESTRANDOMIZER", - "Tschirnhaus", - "classname", "denormalize", "denormalized", + "DWITH_INTEGRATION_TEST", "engageable", "euclidian", "hakuturu", + "Kataoka", "libunwind", + "linelint", + "Masaya", + "Mersenne", + "Monic", + "Moszynski", "npcs", + "NSPACES", "okta", "oktas", "oneof", "parametersetaction", + "Petrich", "piotr", "pluggable", + "PREDEF", "protos", "pyproject", "randomizer", @@ -39,6 +40,7 @@ "subspline", "TESTRANDOMIZER", "travelling", + "Tschirnhaus", "xerces", "xercesc", "yamacir-kit" diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 694a75f3b3f..bbbeb7c3384 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -54,19 +54,28 @@ The release branches are used only to update the release notes. An example is [h Your changes proposed in your pull request will be tested automatically by the following checks: -| Checks | Description | -|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------| -| [![BuildAndRun](https://github.com/tier4/scenario_simulator_v2/actions/workflows/BuildAndRun.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/BuildAndRun.yaml) | Build each package independently, run linters, and run unit tests and scenario test. | -| [![Docker](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Docker.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Docker.yaml) | Build a docker image. | -| [![Documentation](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml) | Build the documentation sites. | -| [![SpellCheck](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml) | Run a spell checker and add warnings to the PR. | +| Checks | Description | +| --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------ | +| [![BuildAndRun](https://github.com/tier4/scenario_simulator_v2/actions/workflows/BuildAndRun.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/BuildAndRun.yaml) | Build each package independently, run linters, unit tests and scenario tests. | +| [![CheckBranchUpToDate](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckBranchUpToDate.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckBranchUpToDate.yaml) | Checking the branch is up to date. This workflow works on merge queue. | +| [![CheckLabel](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckLabel.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckLabel.yaml) | Checking the label for version control is labeled. | +| [![Docker](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Docker.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Docker.yaml) | Build a docker image. | +| [![Documentation](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml) | Build the documentation sites. | +| [![DocumentationLinkCheck](https://github.com/tier4/scenario_simulator_v2/actions/workflows/DocumentationLinkCheck.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/DocumentationLinkCheck.yaml) | Checking the URLs in documentation are valid. | +| [![LineLint](https://github.com/tier4/scenario_simulator_v2/actions/workflows/LineLint.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/LineLint.yaml) | Checking text files contain the blank line at the end of the files. | +| [![Release](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Release.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Release.yaml) | Bump new version and create a release. This workflow works on merge queue. | +| [![SpellCheck](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml) | Run a spell checker and add warnings to the PR. | If you contribute to the documentation, your changes should pass the checks below: -| Checks | Description | -|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-------------------------------------------------| -| [![Documentation](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml) | Build the documentation sites. | -| [![SpellCheck](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml) | Run a spell checker and add warnings to the PR. | +| Checks | Description | +| --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------ | +| [![CheckBranchUpToDate](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckBranchUpToDate.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckBranchUpToDate.yaml) | Checking the branch is up to date. This workflow works on merge queue. | +| [![CheckLabel](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckLabel.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/CheckLabel.yaml) | Checking the label for version control is labeled. | +| [![Documentation](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Documentation.yaml) | Build the documentation sites. | +| [![DocumentationLinkCheck](https://github.com/tier4/scenario_simulator_v2/actions/workflows/DocumentationLinkCheck.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/DocumentationLinkCheck.yaml) | Checking the URLs in documentation are valid. | +| [![Release](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Release.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/Release.yaml) | Bump new version and create a release. This workflow works on merge queue. | +| [![SpellCheck](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml/badge.svg)](https://github.com/tier4/scenario_simulator_v2/actions/workflows/SpellCheck.yaml) | Run a spell checker and add warnings to the PR. | ## Code review @@ -76,9 +85,19 @@ As a good practice, reply to the reviewer's comment with a link to your changes To keep the commit hashes consistent, **please DO NOT force-push the commit to your pull request during the code review.** If you want to force-push the commit during the review, please contact the maintainers for approval in advance. -If more than one maintainer approves your pull request and all checks are passed, your pull request will be merged into the `master` branch. +If at least one maintainer approves your pull request and all checks are passed, your pull request will be merged into the `master` branch. Your contribution will be recorded in the [release note](https://tier4.github.io/scenario_simulator_v2-docs/ReleaseNotes/). +Pull requests must be labeled `bump major`, `bump minor` or `bump patch`. + +Please follow the criteria below to determine which label to apply. + +* If there is a destructive change to the scenario or traffic_simulator API, label it label it with `bump major`. +* If the scenario or traffic_simulator API has some additional functionality but no destructive changes, label it with `bump minor`. +* If there is no additional functionality and full backward compatibility is ensured, label it with `bump patch`. + +If you are in any doubt, please consult the maintainers [@hakuturu583](https://github.com/hakuturu583),[@yamacir-kit](https://github.com/yamacir-kit),[@HansRobo](https://github.com/HansRobo). + ## Update Documentation If your pull request changes the behavior of the software, please update the documentation in the `docs/` directory. diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 16478b074f0..e9630547d50 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package arithmetic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index b509f34a56c..0b14398bdbd 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 0.8.0 + 1.0.0 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 b3542d2fdc3..1fe18b5a4db 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -2,6 +2,229 @@ Changelog for package geometry ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge pull request `#1139 `_ from tier4/fix/geometry-bug-fixes +* Revert "Remove tests that do not pass" +* Merge branch 'fix/get-polygon-0-points' into fix/geometry-bug-fixes +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Fix getPolygon 0 points bug +* Merge remote-tracking branch 'origin/master' into feature/traffic-lights-awsim-support +* Merge pull request `#1147 `_ from RobotecAI/feature/test-geometry +* Remove tests that do not pass +* Remove comments +* Merge branch 'feature/test-geometry-spline-subspline' into feature/test-geometry +* Remove empty test +* Fix CatmullRomSpline getPolygon test +* Fix linear algebra divide_zero test +* Fix HermiteCurve trajectory tests +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Fix some incorrect tests +* Fix missing lambda argument +* Correct subspline collision point calculation +* Fix subspline collision point calculation +* Refactor and add getCollisionPointsIn2D function +* Correct spline tests for length estimation inaccuracy +* Merge branch 'fix/spline-max-2d-curvature' into fix/minor-bug-fixes +* Merge branch 'fix/spline-collision-bug' into fix/minor-bug-fixes +* Merge branch 'fix/remove-equal-operators' into fix/minor-bug-fixes +* Merge branch 'fix/polygon-get-min-max-value-bug' into fix/minor-bug-fixes +* Merge branch 'fix/line-segment-initialization' into fix/minor-bug-fixes +* Merge branch 'fix/intersection-vector-bug' into fix/minor-bug-fixes +* Correct CatmullRomSpline tests +* Correct literals + refactor spline & subspline tests +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Fix spline collision point calculation +* Fix spline maximum 2D curvature calculation +* Fix getTrajectory wrong number of points +* Fix getMinValue and getMaxValue empty vector bug +* Fix LineSegment initialization bug +* Fix intersection vector lookup after the last element +* Remove Point & Vector3 equal operators +* Correct literals and refactor + clean code in tests +* Clean test CMakeLists.txt +* Revert "Change test include directory: relative -> absolute" +* Change test include directory: relative -> absolute +* Add and correct LineSegment test cases +* Adjust LineSegment tests to new changes +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Adjust subspline tests to use test_utils.hpp +* Merge branch 'feature/test-geometry' into spline-subspline-tests +* Move test helper functions into common header file +* Correct exception type in Polygon.getMaxValueEmptyVector and Polygon.getMinValueEmptyVector +* Add missing CatmullRomSubspline tests +* Add missing CatmullRomSpline tests +* Correct LineSegment.initializeVectorZero test to expect error +* Refactor Polygon tests +* Refactor Intersection tests - remove loop +* Refactor Collision tests - add functions +* Refactor Transform tests - add functions +* Refactor LinearAlgebra tests - add functions +* Refactor Distance tests - add functions +* Refactor BoundingBox tests - add functions +* Add Vector3 tests +* Correct HermiteCurve tests +* Refactor CatmullRomSpline tests +* Add missing HermiteCurve tests +* Add missing Polygon tests +* Refactor HermiteCurve tests +* Refactor Polygon tests +* Add missing LineSegment tests +* Add missing Intersection tests +* Add missing Collision tests +* Add missing Transform tests +* Correct LinearAlgebra tests +* Add missing LinearAlgebra tests +* Add missing Distance tests +* Add missing BoundingBox tests +* Merge remote-tracking branch 'origin/master' into fix/rtc_command_action/continuous_execution +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge pull request `#1102 `_ from tier4/fix/wrong_distance +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into fix/port_document +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* Change subtraction to assignment +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1019 `_ from tier4/feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge branch 'master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge pull request `#1095 `_ from tier4/feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* code refactor +* code refactor +* implement freespace for relative distance condition +* Init working version of DistanceCondition freespace +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* fix case +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* fix format +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* remove workbound for galactic +* fix typo +* add torelance +* fix getSValue function in line segment class +* add getSValue function +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* use auto +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* add member initializer +* add test case +* fix problem in total_length in CatmullRomSpline class +* use push_back +* add comment +* update comment +* add comment +* fix comment +* fix typo +* add white line +* add white line +* fix typo +* fix typo +* remove plot +* add comment +* udpate comment +* add comment +* add comment +* apply reformat +* update comment +* add gnupolot files +* update comment +* fix comment +* update if +* add comment +* add comment +* add auto scale +* add test case +* fix denormalize +* add comment +* add comment +* add comment +* use auto and -> +* use & +* use auto -> +* remove unused header +* use auto and -> +* add const +* update comment +* add description +* care edge case +* simplify code +* update comment +* update comment +* update comment +* fix typo +* add error message +* add comment +* add test case +* add comment +* add comment +* add comment +* add test case +* update test case +* remove unused function +* add autoscale to the line segment +* update description +* add description +* fix autoscale feature +* add comment +* fix typo +* enable fallback for each functions +* fix some of member function +* add white line for visibility +* add message +* add comment +* remove unused line +* add comment +* add comments +* add comment +* add comment +* add comment +* add test case +* add test case +* add docs +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* add test case +* add test case +* add test case +* fix calculate s value +* fix getLineSegments function +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* enable check collision to the point +* add functions +* simplify code +* add const +* use lambda +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Shota Minami, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge pull request `#1058 `_ from tier4/ref/RJD-553_restore_repeated_update_entity_status diff --git a/common/math/geometry/include/geometry/linear_algebra.hpp b/common/math/geometry/include/geometry/linear_algebra.hpp index 8a5e7f7dcc8..c52ec730b53 100644 --- a/common/math/geometry/include/geometry/linear_algebra.hpp +++ b/common/math/geometry/include/geometry/linear_algebra.hpp @@ -50,6 +50,4 @@ geometry_msgs::msg::Vector3 operator-( const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1); geometry_msgs::msg::Point operator-( const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1); -bool operator==(const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1); -bool operator==(const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1); #endif // GEOMETRY__LINEAR_ALGEBRA_HPP_ diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp index d5eb860c2cb..0c9d47bec1d 100644 --- a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp +++ b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp @@ -49,6 +49,9 @@ class CatmullRomSpline : public CatmullRomSplineInterface -> double; auto getSquaredDistanceVector(const geometry_msgs::msg::Point & point, const double s) const -> geometry_msgs::msg::Vector3; + auto getCollisionPointsIn2D( + const std::vector & polygon, + const bool search_backward = false) const -> std::set; auto getCollisionPointIn2D( const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1, const bool search_backward = false) const -> std::optional; diff --git a/common/math/geometry/include/geometry/spline/hermite_curve.hpp b/common/math/geometry/include/geometry/spline/hermite_curve.hpp index e4966b6b493..a1afdb4f0c7 100644 --- a/common/math/geometry/include/geometry/spline/hermite_curve.hpp +++ b/common/math/geometry/include/geometry/spline/hermite_curve.hpp @@ -63,12 +63,18 @@ class HermiteCurve const geometry_msgs::msg::Point & point, double s, bool denormalize_s = false) const; geometry_msgs::msg::Vector3 getSquaredDistanceVector( const geometry_msgs::msg::Point & point, double s, bool denormalize_s = false) const; + std::set getCollisionPointsIn2D( + const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1, + bool search_backward = false, bool denormalize_s = false) const; std::optional getCollisionPointIn2D( const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1, - bool search_backward = false) const; + bool search_backward = false, bool denormalize_s = false) const; + std::set getCollisionPointsIn2D( + const std::vector & polygon, bool search_backward = false, + bool close_start_end = true, bool denormalize_s = false) const; std::optional getCollisionPointIn2D( const std::vector & polygon, bool search_backward = false, - bool close_start_end = true) const; + bool close_start_end = true, bool denormalize_s = false) const; private: std::pair get2DMinMaxCurvatureValue() const; diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index c3deb9b3604..6d7209930c0 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 0.8.0 + 1.0.0 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 1f4311c24b3..07a32df0b9a 100644 --- a/common/math/geometry/src/intersection/intersection.cpp +++ b/common/math/geometry/src/intersection/intersection.cpp @@ -41,8 +41,8 @@ bool isIntersect2D(const LineSegment & line0, const LineSegment & line1) bool isIntersect2D(const std::vector & lines) { - for (size_t i = 0; i <= lines.size(); i++) { - for (size_t m = 0; m <= lines.size(); m++) { + for (size_t i = 0; i < lines.size(); ++i) { + for (size_t m = 0; m < lines.size(); ++m) { if (i != m && isIntersect2D(lines[i], lines[m])) { return true; } @@ -74,8 +74,8 @@ std::optional getIntersection2D( std::vector getIntersection2D(const std::vector & lines) { std::vector ret; - for (size_t i = 0; i <= lines.size(); i++) { - for (size_t m = 0; m <= lines.size(); m++) { + for (size_t i = 0; i < lines.size(); ++i) { + for (size_t m = 0; m < lines.size(); ++m) { if (i != m) { const auto point = getIntersection2D(lines[i], lines[m]); if (point) { diff --git a/common/math/geometry/src/linear_algebra.cpp b/common/math/geometry/src/linear_algebra.cpp index 370b025f11b..0db41afa163 100644 --- a/common/math/geometry/src/linear_algebra.cpp +++ b/common/math/geometry/src/linear_algebra.cpp @@ -149,21 +149,3 @@ geometry_msgs::msg::Point operator-( ret.z = v0.z - v1.z; return ret; } - -bool operator==(const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1) -{ - constexpr double e = std::numeric_limits::epsilon(); - if (std::fabs(v0.x - v1.x) <= e && std::fabs(v0.y - v1.y) <= e && std::fabs(v0.z - v1.z) <= e) { - return true; - } - return false; -} - -bool operator==(const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1) -{ - constexpr double e = std::numeric_limits::epsilon(); - if (std::fabs(v0.x - v1.x) <= e && std::fabs(v0.y - v1.y) <= e && std::fabs(v0.z - v1.z) <= e) { - return true; - } - return false; -} diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp index 9cf0022e7f3..5bc4f67e9fe 100644 --- a/common/math/geometry/src/polygon/line_segment.cpp +++ b/common/math/geometry/src/polygon/line_segment.cpp @@ -39,6 +39,13 @@ LineSegment::LineSegment( : 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) { + 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."); + } 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; diff --git a/common/math/geometry/src/polygon/polygon.cpp b/common/math/geometry/src/polygon/polygon.cpp index 59da4c1ef7d..a72d9d55ba5 100644 --- a/common/math/geometry/src/polygon/polygon.cpp +++ b/common/math/geometry/src/polygon/polygon.cpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace math { @@ -51,13 +52,41 @@ std::vector get2DConvexHull( double getMaxValue(const std::vector & points, const Axis & axis) { + if (points.empty()) { + THROW_SIMULATION_ERROR( + "Invalid point list is specified, while getting max value on ", + axis == Axis::X ? "X" + : axis == Axis::Y ? "Y" + : "Z", + " axis. ", + "The point list in getMaxValue should have at least one point to get the max value from. " + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } const auto values = filterByAxis(points, axis); + if (values.size() == 1) { + return values.front(); + } return *std::max_element(values.begin(), values.end()); } double getMinValue(const std::vector & points, const Axis & axis) { + if (points.empty()) { + THROW_SIMULATION_ERROR( + "Invalid point list is specified, while getting min value on ", + axis == Axis::X ? "X" + : axis == Axis::Y ? "Y" + : "Z", + " axis. ", + "The point list in getMinValue should have at least one point to get the min value from. " + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } const auto values = filterByAxis(points, axis); + if (values.size() == 1) { + return values.front(); + } return *std::min_element(values.begin(), values.end()); } diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index bef2359147a..baf7a66eaf9 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -31,6 +31,9 @@ auto CatmullRomSpline::getPolygon( const double width, const size_t num_points, const double z_offset) -> std::vector { + if (num_points == 0) { + return {}; + } std::vector points; std::vector left_bounds = getLeftBounds(width, num_points, z_offset); std::vector right_bounds = getRightBounds(width, num_points, z_offset); @@ -278,15 +281,9 @@ auto CatmullRomSpline::getSInSplineCurve(const size_t curve_index, const double THROW_SEMANTIC_ERROR("curve index does not match"); // LCOV_EXCL_LINE } -/** - * @brief Get collision point in 2D (x and y) - * @param polygon points of polygons. - * @param search_backward If true, return collision points with maximum s value. If false, return collision points with minimum s value. - * @return std::optional Denormalized s values along the frenet coordinate of the spline curve. - */ -auto CatmullRomSpline::getCollisionPointIn2D( +auto CatmullRomSpline::getCollisionPointsIn2D( const std::vector & polygon, const bool search_backward) const - -> std::optional + -> std::set { if (polygon.size() <= 1) { THROW_SIMULATION_ERROR( @@ -297,32 +294,20 @@ auto CatmullRomSpline::getCollisionPointIn2D( } /// @note If the spline has three or more control points. const auto get_collision_point_2d_with_curve = - [this](const auto & polygon, const auto search_backward) -> std::optional { - size_t n = curves_.size(); - if (search_backward) { - for (size_t i = 0; i < n; i++) { - auto s = curves_[n - 1 - i].getCollisionPointIn2D(polygon, search_backward); - if (s) { - return getSInSplineCurve(n - 1 - i, s.value()); - } - } - return std::optional(); - } else { - for (size_t i = 0; i < n; i++) { - auto s = curves_[i].getCollisionPointIn2D(polygon, search_backward); - if (s) { - return std::optional(getSInSplineCurve(i, s.value())); - } - } - return std::optional(); + [this](const auto & polygon, const auto search_backward) -> std::set { + std::set s_value_candidates; + for (size_t i = 0; i < curves_.size(); ++i) { + /// @note The polygon is assumed to be closed + const auto s = curves_[i].getCollisionPointsIn2D(polygon, search_backward, true, true); + std::for_each(s.begin(), s.end(), [&s_value_candidates, i, this](const auto & s) { + s_value_candidates.insert(getSInSplineCurve(i, s)); + }); } - return std::optional(); + return s_value_candidates; }; /// @note If the spline has two control points. (Same as single line segment.) - const auto get_collision_point_2d_with_line = - [this](const auto & polygon, const auto search_backward) -> std::optional { - const auto polygon_lines = getLineSegments(polygon); - std::vector s_value_candidates = {}; + const auto get_collision_point_2d_with_line = [this](const auto & polygon) -> std::set { + std::set s_value_candidates; for (const auto & line : getLineSegments(polygon)) { if (static_cast(line_segments_.size()) != 1) { THROW_SIMULATION_ERROR( @@ -332,26 +317,21 @@ auto CatmullRomSpline::getCollisionPointIn2D( "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } - if (const auto s_value = line_segments_[0].getIntersection2DSValue(line)) { - s_value_candidates.push_back(s_value.value()); + if (const auto s = line_segments_[0].getIntersection2DSValue(line, true)) { + s_value_candidates.insert(s.value()); } } - if (s_value_candidates.empty()) { - return std::optional(); - } - return search_backward - ? *std::max_element(s_value_candidates.begin(), s_value_candidates.end()) - : *std::min_element(s_value_candidates.begin(), s_value_candidates.end()); + return s_value_candidates; }; /// @note If the spline has one control point. (Same as point.) - const auto get_collision_point_2d_with_point = [this](const auto & polygon) { - const auto polygon_lines = getLineSegments(polygon); - for (const auto & line : polygon_lines) { + const auto get_collision_point_2d_with_point = [this](const auto & polygon) -> std::set { + std::set s_value_candidates; + for (const auto & line : getLineSegments(polygon)) { if (line.isIntersect2D(control_points[0])) { - return std::optional(0); + s_value_candidates.insert(0.0); } } - return std::optional(); + return s_value_candidates; }; switch (control_points.size()) { @@ -366,13 +346,33 @@ auto CatmullRomSpline::getCollisionPointIn2D( return get_collision_point_2d_with_point(polygon); /// @note In this case, spline is interpreted as line segment. case 2: - return get_collision_point_2d_with_line(polygon, search_backward); + return get_collision_point_2d_with_line(polygon); /// @note In this case, spline is interpreted as curve. default: return get_collision_point_2d_with_curve(polygon, search_backward); } } +/** + * @brief Get collision point in 2D (x and y) + * @param polygon points of polygons. + * @param search_backward If true, return collision points with maximum s value. If false, return collision points with minimum s value. + * @return std::optional Denormalized s values along the frenet coordinate of the spline curve. + */ +auto CatmullRomSpline::getCollisionPointIn2D( + const std::vector & polygon, const bool search_backward) const + -> std::optional +{ + std::set s_value_candidates = getCollisionPointsIn2D(polygon, search_backward); + if (s_value_candidates.empty()) { + return std::nullopt; + } + if (search_backward) { + return *(s_value_candidates.rbegin()); + } + return *(s_value_candidates.begin()); +} + auto CatmullRomSpline::getCollisionPointIn2D( const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1, const bool search_backward) const -> std::optional @@ -380,7 +380,7 @@ auto CatmullRomSpline::getCollisionPointIn2D( size_t n = curves_.size(); if (search_backward) { for (size_t i = 0; i < n; i++) { - auto s = curves_[n - 1 - i].getCollisionPointIn2D(point0, point1, search_backward); + auto s = curves_[n - 1 - i].getCollisionPointIn2D(point0, point1, search_backward, true); if (s) { return getSInSplineCurve(n - 1 - i, s.value()); } @@ -388,7 +388,7 @@ auto CatmullRomSpline::getCollisionPointIn2D( return std::nullopt; } else { for (size_t i = 0; i < n; i++) { - auto s = curves_[i].getCollisionPointIn2D(point0, point1, search_backward); + auto s = curves_[i].getCollisionPointIn2D(point0, point1, search_backward, true); if (s) { return getSInSplineCurve(i, s.value()); } @@ -567,7 +567,12 @@ auto CatmullRomSpline::getMaximum2DCurvature() const -> double if (maximum_2d_curvatures_.empty()) { THROW_SIMULATION_ERROR("maximum 2D curvature vector size is 0."); // LCOV_EXCL_LINE } - return *std::max_element(maximum_2d_curvatures_.begin(), maximum_2d_curvatures_.end()); + const auto [min, max] = + std::minmax_element(maximum_2d_curvatures_.begin(), maximum_2d_curvatures_.end()); + if (std::fabs(*min) > std::fabs(*max)) { + return *min; + } + return *max; } auto CatmullRomSpline::getNormalVector(const double s) const -> geometry_msgs::msg::Vector3 diff --git a/common/math/geometry/src/spline/catmull_rom_subspline.cpp b/common/math/geometry/src/spline/catmull_rom_subspline.cpp index 856081509b8..8cfadb4ad24 100644 --- a/common/math/geometry/src/spline/catmull_rom_subspline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_subspline.cpp @@ -14,6 +14,7 @@ #include #include +#include #include namespace math @@ -25,17 +26,40 @@ double CatmullRomSubspline::getLength() const { return end_s_ - start_s_; } std::optional CatmullRomSubspline::getCollisionPointIn2D( const std::vector & polygon, const bool search_backward) const { - auto s = spline_->getCollisionPointIn2D(polygon, search_backward); + /// @note Make sure end is greater than start, otherwise the spline is invalid + if (end_s_ < start_s_) { + THROW_SIMULATION_ERROR( + "The start of the subspline is greater than the end. " + "The start of the subspline should always be less than the end. ", + "Subspline start: ", start_s_, " Subspline end: ", end_s_, " ", + "Something completely unexpected happened. ", + "This message is not originally intended to be displayed, if you see it, please " + "contact the developer of traffic_simulator."); + } + + std::set s_value_candidates = spline_->getCollisionPointsIn2D(polygon); - if (!s) { + if (s_value_candidates.empty()) { return std::nullopt; } - if (s.value() < start_s_ || end_s_ < s.value()) { + /// @note Iterators for the range of this subspline + auto begin = s_value_candidates.lower_bound(start_s_); + auto end = s_value_candidates.upper_bound(end_s_); + + /** + * @note If begin == end there is no collision in the given range, or it is past the range + * If begin == s_value_candidates.end() all elements are less than start_s_ + * end == s_value_candidates.end() is valid, all elements are less than end_s_ + */ + if (begin == end || begin == s_value_candidates.end()) { return std::nullopt; } - return s.value() - start_s_; + if (search_backward) { + return *(--end) - start_s_; // end is past the last element, so we need the one before + } + return *begin - start_s_; } } // namespace geometry } // namespace math diff --git a/common/math/geometry/src/spline/hermite_curve.cpp b/common/math/geometry/src/spline/hermite_curve.cpp index c32d676fd10..a06eb25b530 100644 --- a/common/math/geometry/src/spline/hermite_curve.cpp +++ b/common/math/geometry/src/spline/hermite_curve.cpp @@ -85,45 +85,47 @@ geometry_msgs::msg::Vector3 HermiteCurve::getSquaredDistanceVector( return ret; } -std::optional HermiteCurve::getCollisionPointIn2D( +std::set HermiteCurve::getCollisionPointsIn2D( const std::vector & polygon, bool search_backward, - bool close_start_end) const + bool close_start_end, bool denormalize_s) const { size_t n = polygon.size(); if (n <= 1) { - return std::nullopt; + return {}; } - std::vector s_values; + std::set s_values; for (size_t i = 0; i < (n - 1); i++) { const auto p0 = polygon[i]; const auto p1 = polygon[i + 1]; - auto s = getCollisionPointIn2D(p0, p1, search_backward); - if (s) { - s_values.push_back(s.value()); - } + s_values.merge(getCollisionPointsIn2D(p0, p1, search_backward, denormalize_s)); } if (close_start_end) { const auto p0 = polygon[n - 1]; const auto p1 = polygon[0]; - auto s = getCollisionPointIn2D(p0, p1, search_backward); - if (s) { - s_values.push_back(s.value()); - } + s_values.merge(getCollisionPointsIn2D(p0, p1, search_backward, denormalize_s)); } + return s_values; +} + +std::optional HermiteCurve::getCollisionPointIn2D( + const std::vector & polygon, bool search_backward, + bool close_start_end, bool denormalize_s) const +{ + auto s_values = getCollisionPointsIn2D(polygon, search_backward, close_start_end, denormalize_s); if (s_values.empty()) { return std::nullopt; } if (search_backward) { - return *std::max_element(s_values.begin(), s_values.end()); + return *s_values.rbegin(); } - return *std::min_element(s_values.begin(), s_values.end()); + return *s_values.begin(); } -std::optional HermiteCurve::getCollisionPointIn2D( +std::set HermiteCurve::getCollisionPointsIn2D( const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1, - bool search_backward) const + bool search_backward, bool denormalize_s) const { - std::vector s_values; + std::set s_values; double fx = point0.x; double ex = (point1.x - point0.x); double fy = point0.y; @@ -151,6 +153,16 @@ std::optional HermiteCurve::getCollisionPointIn2D( } }; + /** + * @note Denormalize given S value as necessary + */ + const auto denormalize = [denormalize_s, this](double s) -> double { + if (denormalize_s) { + return s * getLength(); + } + return s; + }; + for (const auto solution : get_solutions()) { constexpr double epsilon = std::numeric_limits::epsilon(); double x = solver_.cubic(ax_, bx_, cx_, dx_, solution); @@ -163,21 +175,29 @@ std::optional HermiteCurve::getCollisionPointIn2D( * tx, ty, will be in the range [0, 1] while the other will be out of that range because of division by zero. */ if ((0 <= tx && tx <= 1) || (0 <= ty && ty <= 1)) { - s_values.push_back(solution); + s_values.insert(denormalize(solution)); } } else { if ((0 <= tx && tx <= 1) && (0 <= ty && ty <= 1)) { - s_values.push_back(solution); + s_values.insert(denormalize(solution)); } } } + return s_values; +} + +std::optional HermiteCurve::getCollisionPointIn2D( + const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1, + bool search_backward, bool denormalize_s) const +{ + auto s_values = getCollisionPointsIn2D(point0, point1, search_backward, denormalize_s); if (s_values.empty()) { return std::nullopt; } if (search_backward) { - return *std::max_element(s_values.begin(), s_values.end()); + return *s_values.rbegin(); } - return *std::min_element(s_values.begin(), s_values.end()); + return *s_values.begin(); } std::optional HermiteCurve::getSValue( @@ -223,8 +243,12 @@ const std::vector HermiteCurve::getTrajectory( std::vector HermiteCurve::getTrajectory(size_t num_points) const { std::vector ret; - for (size_t i = 0; i <= num_points; i++) { - double t = static_cast(i) / static_cast(num_points); + if (num_points == 1) { // safe check to not divide by zero in the loop + ret.emplace_back(getPoint(0.0, false)); + return ret; + } + for (size_t i = 0; i < num_points; ++i) { + double t = static_cast(i) / static_cast(num_points - 1); ret.emplace_back(getPoint(t, false)); } return ret; diff --git a/common/math/geometry/test/CMakeLists.txt b/common/math/geometry/test/CMakeLists.txt index 416d1d32f35..71741d743ea 100644 --- a/common/math/geometry/test/CMakeLists.txt +++ b/common/math/geometry/test/CMakeLists.txt @@ -1,18 +1,26 @@ +add_subdirectory(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_line_segment test_line_segment.cpp) ament_add_gtest(test_linear_algebra test_linear_algebra.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) 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) target_link_libraries(test_distance geometry) target_link_libraries(test_hermite_curve geometry) -target_link_libraries(test_line_segment geometry) target_link_libraries(test_linear_algebra geometry) target_link_libraries(test_polygon geometry) target_link_libraries(test_polynomial_solver geometry) +target_link_libraries(test_transform geometry) +target_link_libraries(test_intersection geometry) +target_link_libraries(test_line_segment geometry) diff --git a/common/math/geometry/test/expect_eq_macros.hpp b/common/math/geometry/test/expect_eq_macros.hpp index c89552dcd88..5b452f67385 100644 --- a/common/math/geometry/test/expect_eq_macros.hpp +++ b/common/math/geometry/test/expect_eq_macros.hpp @@ -28,21 +28,50 @@ EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); +#define EXPECT_BOOST_POINT_2D_AND_POINT_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x(), DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y(), DATA1.y); + +#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); + +#define EXPECT_POINT_NAN(DATA) \ + EXPECT_TRUE(std::isnan(DATA.x)); \ + EXPECT_TRUE(std::isnan(DATA.y)); \ + EXPECT_TRUE(std::isnan(DATA.z)); + #define EXPECT_VECTOR3_EQ(DATA0, DATA1) \ EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); +#define EXPECT_VECTOR3_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); + #define EXPECT_QUATERNION_EQ(DATA0, DATA1) \ EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); +#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); \ + EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE); + #define EXPECT_POSE_EQ(DATA0, DATA1) \ EXPECT_POINT_EQ(DATA0.position, DATA1.position); \ EXPECT_QUATERNION_EQ(DATA0.orientation, DATA1.orientation); +#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \ + EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE); + #define EXPECT_LANELET_POSE_EQ(DATA0, DATA1) \ EXPECT_EQ(DATA0.lanelet_id, DATA1.lanelet_id); \ EXPECT_DOUBLE_EQ(DATA0.s, DATA1.s); \ diff --git a/common/math/geometry/test/test_bounding_box.cpp b/common/math/geometry/test/test_bounding_box.cpp index 13243e503ee..5184b787dcc 100644 --- a/common/math/geometry/test/test_bounding_box.cpp +++ b/common/math/geometry/test/test_bounding_box.cpp @@ -13,40 +13,115 @@ // limitations under the License. #include +#include #include #include -TEST(BoundingBox, GetPolygonDistanceWithCollision) +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +TEST(BoundingBox, getPointsFromBboxDefault) +{ + geometry_msgs::msg::Pose pose; + traffic_simulator_msgs::msg::BoundingBox bbox; + std::vector points = math::geometry::getPointsFromBbox(bbox); + EXPECT_EQ(points.size(), size_t(4)); + EXPECT_POINT_EQ(points[0], makePoint(0.0, 0.0, 0.0)); + EXPECT_POINT_EQ(points[1], makePoint(0.0, 0.0, 0.0)); + EXPECT_POINT_EQ(points[2], makePoint(0.0, 0.0, 0.0)); + EXPECT_POINT_EQ(points[3], makePoint(0.0, 0.0, 0.0)); +} + +TEST(BoundingBox, getPointsFromBboxCustom) +{ + geometry_msgs::msg::Pose pose; + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(5.0, 2.0, 2.0, 1.0); + std::vector points = + math::geometry::getPointsFromBbox(bbox, 1.0, 2.0, 3.0, 4.0); + EXPECT_EQ(points.size(), size_t(4)); + EXPECT_POINT_EQ(points[0], makePoint(6.5, 3.0, 1.0)); + EXPECT_POINT_EQ(points[1], makePoint(-5.5, 3.0, 1.0)); + EXPECT_POINT_EQ(points[2], makePoint(-5.5, -2.0, 1.0)); + EXPECT_POINT_EQ(points[3], makePoint(6.5, -2.0, 1.0)); +} + +TEST(BoundingBox, get2DPolygonZeroPose) +{ + geometry_msgs::msg::Pose pose; + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0); + boost::geometry::model::polygon> poly = + math::geometry::get2DPolygon(pose, bbox); + 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)); +} + +TEST(BoundingBox, get2DPolygonOnlyTranslation) +{ + geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0); + boost::geometry::model::polygon> poly = + math::geometry::get2DPolygon(pose, bbox); + 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)); +} + +TEST(BoundingBox, get2DPolygonFullPose) +{ + geometry_msgs::msg::Pose pose = makePose(1.0, 2.0); + pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0.0).y(0.0).z(30.0 * M_PI / 180.0)); + traffic_simulator_msgs::msg::BoundingBox bbox = makeBbox(2.0, 2.0, 2.0); + boost::geometry::model::polygon> poly = + math::geometry::get2DPolygon(pose, bbox); + EXPECT_EQ(poly.inners().size(), size_t(0)); + EXPECT_EQ(poly.outer().size(), size_t(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)); +} + +TEST(BoundingBox, getPolygonDistanceWithCollision) { geometry_msgs::msg::Pose pose0; - traffic_simulator_msgs::msg::BoundingBox bbox0; - bbox0.dimensions.x = 3; - bbox0.dimensions.y = 3; - bbox0.dimensions.z = 3; + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0); geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox bbox1; - bbox1.dimensions.x = 1; - bbox1.dimensions.y = 1; - bbox1.dimensions.z = 1; + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0); EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt); } -TEST(BoundingBox, GetPolygonDistanceWithoutCollision) +TEST(BoundingBox, getPolygonDistanceTouch) { geometry_msgs::msg::Pose pose0; - traffic_simulator_msgs::msg::BoundingBox bbox0; - bbox0.dimensions.x = 3; - bbox0.dimensions.y = 3; - bbox0.dimensions.z = 3; - geometry_msgs::msg::Pose pose1; - pose1.position.y = 5; - traffic_simulator_msgs::msg::BoundingBox bbox1; - bbox1.dimensions.x = 1; - bbox1.dimensions.y = 1; - bbox1.dimensions.z = 1; - EXPECT_TRUE(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1)); - EXPECT_DOUBLE_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1).value(), 3.0); + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(4.0, 4.0, 4.0); + geometry_msgs::msg::Pose pose1 = makePose(3.0, 3.0, 3.0); + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(2.0, 2.0, 2.0); + EXPECT_EQ(math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1), std::nullopt); +} + +TEST(BoundingBox, getPolygonDistanceWithoutCollision) +{ + geometry_msgs::msg::Pose pose0; + traffic_simulator_msgs::msg::BoundingBox bbox0 = makeBbox(3.0, 3.0, 3.0); + geometry_msgs::msg::Pose pose1 = makePose(0.0, 5.0); + traffic_simulator_msgs::msg::BoundingBox bbox1 = makeBbox(1.0, 1.0, 1.0); + const auto ans = math::geometry::getPolygonDistance(pose0, bbox0, pose1, bbox1); + EXPECT_TRUE(ans); + EXPECT_DOUBLE_EQ(ans.value(), 3.0); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/test_catmull_rom_spline.cpp index dbe97a86955..a3f2976855d 100644 --- a/common/math/geometry/test/test_catmull_rom_spline.cpp +++ b/common/math/geometry/test/test_catmull_rom_spline.cpp @@ -18,6 +18,46 @@ #include #include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-6; + +/// @brief Helper function generating line: p(0,0)-> p(1,3) -> p(2,6) +math::geometry::CatmullRomSpline makeLine() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 6.0)}; + return math::geometry::CatmullRomSpline(points); +} + +/// @brief Helper function generating curve: p(0,0)-> p(1,1)-> p(2,0) +math::geometry::CatmullRomSpline makeCurve() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 1.0), makePoint(2.0, 0.0)}; + return math::geometry::CatmullRomSpline(points); +} + +/// @brief Helper function generating curve: p(0,0)-> p(1,1)-> p(0,2) +math::geometry::CatmullRomSpline makeCurve2() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 1.0), makePoint(0.0, 2.0)}; + return math::geometry::CatmullRomSpline(points); +} + +/** + * Add an offset to the given point in a specified direction. + * + * @param point The point to which the offset will be added. + * @param offset The value of the offset. + * @param theta The angle in radians representing the direction. + */ +void addOffset(geometry_msgs::msg::Point & point, const double offset, const double theta) +{ + point.x += std::cos(theta) * offset; + point.y += std::sin(theta) * offset; +} /// @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. @@ -136,220 +176,441 @@ TEST(CatmullRomSpline, GetCollisionWith1ControlPoint) TEST(CatmullRomSpline, GetCollisionPointIn2D) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - auto points = {p0, p1, p2}; - auto spline = math::geometry::CatmullRomSpline(points); - EXPECT_DOUBLE_EQ(spline.getLength(), 2); - geometry_msgs::msg::Point start; - start.x = 0.1; - start.y = 1.0; - geometry_msgs::msg::Point goal; - goal.x = 0.1; - goal.y = -1.0; - auto collision_s = spline.getCollisionPointIn2D(start, goal, false); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0)}; + const math::geometry::CatmullRomSpline spline(points); + + EXPECT_NEAR(spline.getLength(), 2.0, EPS); + geometry_msgs::msg::Point start = makePoint(0.1, 1.0), goal = makePoint(0.1, -1.0); + + const auto collision_s0 = spline.getCollisionPointIn2D(start, goal, false); + EXPECT_TRUE(collision_s0); + if (collision_s0) { + EXPECT_NEAR(collision_s0.value(), 0.1, EPS); } - collision_s = spline.getCollisionPointIn2D({start, goal}, false); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const auto collision_s1 = spline.getCollisionPointIn2D({start, goal}, false); + EXPECT_TRUE(collision_s1); + if (collision_s1) { + EXPECT_NEAR(collision_s1.value(), 0.1, EPS); } - collision_s = spline.getCollisionPointIn2D(start, goal, true); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const auto collision_s2 = spline.getCollisionPointIn2D(start, goal, true); + EXPECT_TRUE(collision_s2); + if (collision_s2) { + EXPECT_NEAR(collision_s2.value(), 0.1, EPS); } - collision_s = spline.getCollisionPointIn2D({start, goal}, true); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); + const auto collision_s3 = spline.getCollisionPointIn2D({start, goal}, true); + EXPECT_TRUE(collision_s3); + if (collision_s3) { + EXPECT_NEAR(collision_s3.value(), 0.1, EPS); } } -TEST(CatmullRomSpline, Maximum2DCurvature) +TEST(CatmullRomSpline, getCollisionPointIn2D) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - p1.y = 3; - geometry_msgs::msg::Point p2; - p2.x = 2; - p2.y = 6; - auto points = {p0, p1, p2}; + const std::vector points{ + makePoint(0.0, 0.0), makePoint(2.0, 0.0), makePoint(4.0, 0.0)}; + const math::geometry::CatmullRomSpline spline(points); + + const geometry_msgs::msg::Point start0 = makePoint(0.1, 1.0), goal0 = makePoint(0.1, -1.0); + + const auto collision_s00 = spline.getCollisionPointIn2D(start0, goal0, false); + EXPECT_TRUE(collision_s00); + EXPECT_NEAR(collision_s00.value(), 0.1, EPS); + + const auto collision_s01 = spline.getCollisionPointIn2D({start0, goal0}, false); + EXPECT_TRUE(collision_s01); + EXPECT_NEAR(collision_s01.value(), 0.1, EPS); + + const auto collision_s02 = spline.getCollisionPointIn2D(start0, goal0, true); + EXPECT_TRUE(collision_s02); + EXPECT_NEAR(collision_s02.value(), 0.1, EPS); + + const auto collision_s03 = spline.getCollisionPointIn2D({start0, goal0}, true); + EXPECT_TRUE(collision_s03); + EXPECT_NEAR(collision_s03.value(), 0.1, EPS); + + const geometry_msgs::msg::Point start1 = makePoint(3.5, 1.0), goal1 = makePoint(3.5, -1.0); + + const auto collision_s10 = spline.getCollisionPointIn2D(start1, goal1, false); + EXPECT_TRUE(collision_s10); + EXPECT_NEAR(collision_s10.value(), 3.5, EPS); + + const auto collision_s11 = spline.getCollisionPointIn2D({start1, goal1}, false); + EXPECT_TRUE(collision_s11); + EXPECT_NEAR(collision_s11.value(), 3.5, EPS); + + const auto collision_s12 = spline.getCollisionPointIn2D(start1, goal1, true); + EXPECT_TRUE(collision_s12); + EXPECT_NEAR(collision_s12.value(), 3.5, EPS); + + const auto collision_s13 = spline.getCollisionPointIn2D({start1, goal1}, true); + EXPECT_TRUE(collision_s13); + EXPECT_NEAR(collision_s13.value(), 3.5, EPS); +} + +TEST(CatmullRomSpline, getCollisionPointIn2DNoCollision) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + + const geometry_msgs::msg::Point start = makePoint(0.5, 0.0), goal = makePoint(1.5, 0.0); + + const auto collision_s0 = spline.getCollisionPointIn2D(start, goal, false); + EXPECT_FALSE(collision_s0); + + const auto collision_s1 = spline.getCollisionPointIn2D({start, goal}, false); + EXPECT_FALSE(collision_s1); + + const auto collision_s2 = spline.getCollisionPointIn2D(start, goal, true); + EXPECT_FALSE(collision_s2); + + const auto collision_s3 = spline.getCollisionPointIn2D({start, goal}, true); + EXPECT_FALSE(collision_s3); +} + +TEST(CatmullRomSpline, getCollisionPointIn2DPolygon) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + + const std::vector polygon{ + makePoint(0.0, 1.0), makePoint(-1.0, 0.0), makePoint(0.0, -1.0), makePoint(1.0, 0.0)}; + + const auto collision_s0 = spline.getCollisionPointIn2D(polygon); + EXPECT_TRUE(collision_s0); + EXPECT_NEAR(collision_s0.value(), 0.56727227, EPS); + + const auto collision_s1 = spline.getCollisionPointIn2D(polygon, true); + EXPECT_TRUE(collision_s1); + EXPECT_NEAR(collision_s1.value(), 0.56727227, EPS); +} + +TEST(CatmullRomSpline, getCollisionPointIn2DEmpty) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const std::vector polygon; + + EXPECT_THROW(spline.getCollisionPointIn2D(polygon), common::SimulationError); + EXPECT_THROW(spline.getCollisionPointIn2D(polygon, true), common::SimulationError); +} + +TEST(CatmullRomSpline, getMaximum2DCurvatureLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + EXPECT_DOUBLE_EQ(spline.getMaximum2DCurvature(), 0.0); +} + +TEST(CatmullRomSpline, getMaximum2DCurvatureCurve) +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(0.5, 0.5), makePoint(1.0, 0.0), makePoint(2.0, -1.0), + makePoint(3.0, 0.0)}; + const math::geometry::CatmullRomSpline spline(points); + constexpr double eps = 0.1; + EXPECT_NEAR(spline.getMaximum2DCurvature(), -6.0, eps); +} + +TEST(CatmullRomSpline, getPolygon) +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0), makePoint(3.0, 0.0), + makePoint(4.0, 0.0)}; + math::geometry::CatmullRomSpline spline(points); + std::vector polygon = spline.getPolygon(1.0, 4); + + EXPECT_EQ(polygon.size(), static_cast(24)); + // pair of triangles + EXPECT_POINT_NEAR(polygon[0], makePoint(0.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[1], makePoint(0.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[2], makePoint(1.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[3], makePoint(0.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[4], makePoint(1.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[5], makePoint(1.0, 0.5), EPS); + // pair of triangles + EXPECT_POINT_NEAR(polygon[6], makePoint(1.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[7], makePoint(1.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[8], makePoint(2.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[9], makePoint(1.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[10], makePoint(2.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[11], makePoint(2.0, 0.5), EPS); + // pair of triangles + EXPECT_POINT_NEAR(polygon[12], makePoint(2.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[13], makePoint(2.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[14], makePoint(3.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[15], makePoint(2.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[16], makePoint(3.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[17], makePoint(3.0, 0.5), EPS); + // pair of triangles + EXPECT_POINT_NEAR(polygon[18], makePoint(3.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[19], makePoint(3.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[20], makePoint(4.0, 0.5), EPS); + EXPECT_POINT_NEAR(polygon[21], makePoint(3.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[22], makePoint(4.0, -0.5), EPS); + EXPECT_POINT_NEAR(polygon[23], makePoint(4.0, 0.5), EPS); +} + +TEST(CatmullRomSpline, getPolygonEdge) +{ + math::geometry::CatmullRomSpline spline = makeCurve(); + std::vector polygon0 = spline.getPolygon(1.0, 0); + EXPECT_EQ(polygon0.size(), static_cast(0)); + + std::vector polygon1 = spline.getPolygon(1.0, 1); + EXPECT_EQ(polygon1.size(), static_cast(6)); + + std::vector polygon2 = spline.getPolygon(1.0, 2); + EXPECT_EQ(polygon2.size(), static_cast(12)); +} + +TEST(CatmullRomSpline, initializationLine) +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 6.0)}; + EXPECT_NO_THROW(auto spline = math::geometry::CatmullRomSpline(points)); auto spline = math::geometry::CatmullRomSpline(points); - EXPECT_DOUBLE_EQ(spline.getMaximum2DCurvature(), 0); + EXPECT_DOUBLE_EQ(spline.getMaximum2DCurvature(), 0.0); } -TEST(CatmullRomSpline, Interpolate3Points) +TEST(CatmullRomSpline, initializationCurve) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - p1.y = 3; - geometry_msgs::msg::Point p2; - p2.x = 2; - p2.y = 5; - geometry_msgs::msg::Point p3; - p3.x = 4; - p3.y = 6; - auto points = {p0, p1, p2, p3}; + const std::vector points{ + + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 5.0), makePoint(4.0, 6.0), + makePoint(4.0, 1.0)}; EXPECT_NO_THROW(auto spline = math::geometry::CatmullRomSpline(points)); } -TEST(CatmullRomSpline, Interpolate4Points) +TEST(CatmullRomSpline, getLengthLine) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - p1.y = 3; - geometry_msgs::msg::Point p2; - p2.x = 2; - p2.y = 5; - geometry_msgs::msg::Point p3; - p3.x = 4; - p3.y = 6; - geometry_msgs::msg::Point p4; - p4.x = 4; - p4.y = 10; - auto points = {p0, p1, p2, p3, p4}; - EXPECT_NO_THROW(auto spline = math::geometry::CatmullRomSpline(points)); + const math::geometry::CatmullRomSpline spline = makeLine(); + EXPECT_NEAR(spline.getLength(), std::hypot(2.0, 6.0), EPS); } -TEST(CatmullRomSpline, GetPoint) +TEST(CatmullRomSpline, getLengthCurve) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - geometry_msgs::msg::Point p3; - p3.x = 3; - geometry_msgs::msg::Point p4; - p4.x = 4; - auto points = {p0, p1, p2, p3, p4}; - auto spline = math::geometry::CatmullRomSpline(points); - EXPECT_DOUBLE_EQ(spline.getLength(), 4); - auto point = spline.getPoint(3); - EXPECT_DOUBLE_EQ(point.x, 3); - EXPECT_DOUBLE_EQ(point.y, 0); - EXPECT_DOUBLE_EQ(point.z, 0); + const math::geometry::CatmullRomSpline spline = makeCurve(); + constexpr double eps = 0.1; + EXPECT_NEAR(spline.getLength(), 3.0, eps); } -TEST(CatmullRomSpline, GetSValue) +TEST(CatmullRomSpline, getPointLine) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - geometry_msgs::msg::Point p3; - p3.x = 4; - auto points = {p0, p1, p2, p3}; - auto spline = math::geometry::CatmullRomSpline(points); - geometry_msgs::msg::Pose p; - p.position.x = 0.1; - p.position.y = 0; - p.position.z = 0; - const auto result = spline.getSValue(p); + const math::geometry::CatmullRomSpline spline = makeLine(); + auto point = spline.getPoint(std::hypot(1.5, 4.5)); + EXPECT_POINT_NEAR(point, makePoint(1.5, 4.5), EPS); +} + +TEST(CatmullRomSpline, getPointCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto point = spline.getPoint(1.5); + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(point, makePoint(1.0, 1.0), eps); +} + +TEST(CatmullRomSpline, getTangentVectorLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + const auto vec = spline.getTangentVector(1.0); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(1.5, 4.5); + const double norm_ans = std::hypot(ans.x, ans.y, ans.z); + EXPECT_NEAR(vec.x / norm_vec, ans.x / norm_ans, EPS); + EXPECT_NEAR(vec.y / norm_vec, ans.y / norm_ans, EPS); + EXPECT_NEAR(vec.z / norm_vec, ans.z / norm_ans, EPS); +} + +TEST(CatmullRomSpline, getTangentVectorCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto vec = spline.getTangentVector(1.5); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(1.0, 0.0); + constexpr double eps = 0.15; + EXPECT_NEAR(vec.x / norm_vec, ans.x, eps); + EXPECT_NEAR(vec.y / norm_vec, ans.y, eps); + EXPECT_NEAR(vec.z / norm_vec, ans.z, eps); +} + +TEST(CatmullRomSpline, getNormalVectorLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + const auto vec = spline.getNormalVector(1.0); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(-4.5, 1.5); + const double norm_ans = std::hypot(ans.x, ans.y, ans.z); + EXPECT_NEAR(vec.x / norm_vec, ans.x / norm_ans, EPS); + EXPECT_NEAR(vec.y / norm_vec, ans.y / norm_ans, EPS); + EXPECT_NEAR(vec.z / norm_vec, ans.z / norm_ans, EPS); +} + +TEST(CatmullRomSpline, getNormalVectorCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto vec = spline.getNormalVector(1.5); + const double norm_vec = std::hypot(vec.x, vec.y, vec.z); + const auto ans = makeVector(0.0, 1.0); + constexpr double eps = 0.15; + EXPECT_NEAR(vec.x / norm_vec, ans.x, eps); + EXPECT_NEAR(vec.y / norm_vec, ans.y, eps); + EXPECT_NEAR(vec.z / norm_vec, ans.z, eps); +} + +TEST(CatmullRomSpline, getPoseLine) +{ + const math::geometry::CatmullRomSpline spline = makeLine(); + const auto pose = spline.getPose(std::hypot(1.5, 4.5)); + EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5), EPS); + EXPECT_QUATERNION_NEAR( + pose.orientation, + quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, std::atan(3.0))), EPS); +} + +TEST(CatmullRomSpline, getPoseCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto pose = spline.getPose(1.5); + constexpr double eps = 0.02; + EXPECT_POINT_NEAR(pose.position, makePoint(1.0, 1.0), eps); + EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps); +} + +TEST(CatmullRomSpline, getSValue1) +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0), makePoint(4.0, 0.0)}; + const auto spline = math::geometry::CatmullRomSpline(points); + const auto result = spline.getSValue(makePose(0.1, 0.0)); EXPECT_TRUE(result); - EXPECT_TRUE(result.value() > 0.099); - EXPECT_TRUE(result.value() < 0.101); - p.position.x = 10; - p.position.y = 0; - p.position.z = 0; - EXPECT_FALSE(spline.getSValue(p, 3)); + EXPECT_NEAR(result.value(), 0.1, EPS); + EXPECT_FALSE(spline.getSValue(makePose(10.0, 0.0), 3.0)); } -TEST(CatmullRomSpline, GetSValue2) +TEST(CatmullRomSpline, getSValue2) { - geometry_msgs::msg::Point p0; - p0.x = 89122.2; - p0.y = 43364.1; - p0.z = 3.13364; - geometry_msgs::msg::Point p1; - p1.x = 89122.5; - p1.y = 43363.8; - p1.z = 3.13364; - geometry_msgs::msg::Point p2; - p2.x = 89122.8; - p2.y = 43363.4; - p2.z = 3.13364; - geometry_msgs::msg::Point p3; - p3.x = 89123.1; - p3.y = 43363.0; - p3.z = 3.13364; - geometry_msgs::msg::Point p4; - p4.x = 89123.4; - p4.y = 43362.6; - p4.z = 3.13364; - auto points = {p0, p1, p2, p3, p4}; + std::vector points{ + makePoint(89122.2, 43364.1, 3.13364), makePoint(89122.5, 43363.8, 3.13364), + makePoint(89122.8, 43363.4, 3.13364), makePoint(89123.1, 43363.0, 3.13364), + makePoint(89123.4, 43362.6, 3.13364)}; auto spline = math::geometry::CatmullRomSpline(points); - geometry_msgs::msg::Pose p; - p.position.x = 89122.8; - p.position.y = 43363.4; - p.position.z = 3.13364; - p.orientation.x = -0.0159808; - p.orientation.y = -0.00566353; - p.orientation.z = -0.453507; - p.orientation.w = 0.891092; - { - const auto result = spline.getSValue(p); - EXPECT_TRUE(result); - if (result) { - EXPECT_DOUBLE_EQ(result.value(), 0.92433178422155371); - } - } - p.position.x = 89122.5; - p.position.y = 43363.8; - p.position.z = 3.13364; - p.orientation.x = 0.0159365; - p.orientation.y = -0.00578704; - p.orientation.z = -0.446597; - p.orientation.w = 0.894575; - { - const auto result = spline.getSValue(p); - EXPECT_TRUE(result); - if (result) { - EXPECT_DOUBLE_EQ(result.value(), 0.42440442127906564); - } - } + + geometry_msgs::msg::Pose pose0; + pose0.position.x = 89122.8; + pose0.position.y = 43363.4; + pose0.position.z = 3.13364; + pose0.orientation.x = -0.0159808; + pose0.orientation.y = -0.00566353; + pose0.orientation.z = -0.453507; + pose0.orientation.w = 0.891092; + const auto result0 = spline.getSValue(pose0); + EXPECT_TRUE(result0); + EXPECT_DOUBLE_EQ(result0.value(), 0.92433178422155371); + + geometry_msgs::msg::Pose pose1; + pose1.position.x = 89122.5; + pose1.position.y = 43363.8; + pose1.position.z = 3.13364; + pose1.orientation.x = 0.0159365; + pose1.orientation.y = -0.00578704; + pose1.orientation.z = -0.446597; + pose1.orientation.w = 0.894575; + const auto result1 = spline.getSValue(pose1); + EXPECT_TRUE(result1); + EXPECT_DOUBLE_EQ(result1.value(), 0.42440442127906564); } -TEST(CatmullRomSpline, GetTrajectory) +TEST(CatmullRomSpline, getSValueEdge) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Point p1; - p1.x = 1; - geometry_msgs::msg::Point p2; - p2.x = 2; - geometry_msgs::msg::Point p3; - p3.x = 3; - auto points = {p0, p1, p2, p3}; - auto spline = math::geometry::CatmullRomSpline(points); - auto trajectory = spline.getTrajectory(0, 3, 1.0); - EXPECT_EQ(trajectory.size(), static_cast(4)); - EXPECT_DECIMAL_EQ(trajectory[0].x, 0, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[1].x, 1, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[2].x, 2, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[3].x, 3, 0.00001); - trajectory = spline.getTrajectory(3, 0, 1.0); + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto pose = makePose(-2.0, -2.0); + EXPECT_FALSE(spline.getSValue(pose, 1.0)); +} + +TEST(CatmullRomSpline, getSquaredDistanceIn2D) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto distance = spline.getSquaredDistanceIn2D(makePoint(2.0, 2.0), 1.47895776); + EXPECT_NEAR(distance, 2.0, 1e-2); +} + +TEST(CatmullRomSpline, getSquaredDistanceIn2DSamePoint) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto distance = spline.getSquaredDistanceIn2D(makePoint(1.0, 1.0), 1.47895776); + EXPECT_NEAR(distance, 0.0, 1e-2); +} + +TEST(CatmullRomSpline, getSquaredDistanceVector) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto vector = spline.getSquaredDistanceVector(makePoint(2.0, 2.0), 1.47895776); + EXPECT_POINT_NEAR(vector, makeVector(1.0, 1.0), 1e-2); +} + +TEST(CatmullRomSpline, getSquaredDistanceVectorSamePoint) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto vector = spline.getSquaredDistanceVector(makePoint(1.0, 1.0), 1.47895776); + EXPECT_POINT_NEAR(vector, makeVector(0.0, 0.0), 1e-2); +} + +TEST(CatmullRomSpline, getTrajectoryLine) +{ + std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 0.0), makePoint(2.0, 0.0), makePoint(3.0, 0.0)}; + const auto spline = math::geometry::CatmullRomSpline(points); + + const auto trajectory = spline.getTrajectory(0.0, 3.0, 1.0); EXPECT_EQ(trajectory.size(), static_cast(4)); - EXPECT_DECIMAL_EQ(trajectory[0].x, 3, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[1].x, 2, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[2].x, 1, 0.00001); - EXPECT_DECIMAL_EQ(trajectory[3].x, 0, 0.00001); + EXPECT_POINT_NEAR(trajectory[0], makePoint(0.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[1], makePoint(1.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[2], makePoint(2.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[3], makePoint(3.0, 0.0), EPS); + + const auto trajectory_reverse = spline.getTrajectory(3.0, 0.0, 1.0); + EXPECT_EQ(trajectory_reverse.size(), static_cast(4)); + EXPECT_POINT_NEAR(trajectory_reverse[0], makePoint(3.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory_reverse[1], makePoint(2.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory_reverse[2], makePoint(1.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory_reverse[3], makePoint(0.0, 0.0), EPS); +} + +TEST(CatmullRomSpline, getTrajectoryCurve) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + + const auto trajectory = spline.getTrajectory(0.0, 2.957916, 0.5); + EXPECT_POINT_NEAR(trajectory[0], makePoint(0.0, 0.0), EPS); + EXPECT_POINT_NEAR(trajectory[1], makePoint(0.559992, 0.336669), EPS); + EXPECT_POINT_NEAR(trajectory[2], makePoint(0.893292, 0.673338), EPS); + EXPECT_POINT_NEAR(trajectory[3], makePoint(0.999898, 1.010091), EPS); + EXPECT_POINT_NEAR(trajectory[4], makePoint(0.87779, 1.349586), EPS); + EXPECT_POINT_NEAR(trajectory[5], makePoint(0.525168, 1.68908), EPS); + EXPECT_POINT_NEAR(trajectory[6], makePoint(0.0, 2.0), EPS); + + const auto trajectory_offset = spline.getTrajectory(0.0, 2.957916, 0.5, -1.0); + EXPECT_POINT_NEAR(trajectory_offset[0], makePoint(0.447214, -0.894427), EPS); + EXPECT_POINT_NEAR(trajectory_offset[1], makePoint(1.161918, -0.461883), EPS); + EXPECT_POINT_NEAR(trajectory_offset[2], makePoint(1.730462, 0.126395), EPS); + EXPECT_POINT_NEAR(trajectory_offset[3], makePoint(1.999695, 1.030269), EPS); + EXPECT_POINT_NEAR(trajectory_offset[4], makePoint(1.697341, 1.922592), EPS); + EXPECT_POINT_NEAR(trajectory_offset[5], makePoint(1.112457, 2.498458), EPS); + EXPECT_POINT_NEAR(trajectory_offset[6], makePoint(0.447213, 2.894428), EPS); +} + +TEST(CatmullRomSpline, getTrajectoryEmpty) +{ + const math::geometry::CatmullRomSpline spline = makeCurve2(); + const auto trajectory = spline.getTrajectory(0.0, 1.47895776, 2.0); + constexpr double eps = 1e-2; + EXPECT_EQ(trajectory.size(), static_cast(2)); + EXPECT_POINT_NEAR(trajectory[0], makePoint(0.0, 0.0), eps); + EXPECT_POINT_NEAR(trajectory[1], makePoint(1.0, 1.0), eps); } -TEST(CatmullRomSpline, CheckThrowingErrorWhenTheControlPointsAreNotEnough) +TEST(CatmullRomSpline, initializationNotEnoughControlPoints) { - EXPECT_THROW( - math::geometry::CatmullRomSpline(std::vector(0)), - common::SemanticError); + const std::vector points; + EXPECT_THROW(math::geometry::CatmullRomSpline{points}, common::SemanticError); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_catmull_rom_subspline.cpp b/common/math/geometry/test/test_catmull_rom_subspline.cpp new file mode 100644 index 00000000000..567d1bf089e --- /dev/null +++ b/common/math/geometry/test/test_catmull_rom_subspline.cpp @@ -0,0 +1,171 @@ +// 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 "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-6; + +/// @brief Helper function generating line: p(0,0)-> p(1,3) -> p(2,6) +std::shared_ptr makeLine() +{ + const std::vector points{ + makePoint(0.0, 0.0), makePoint(1.0, 3.0), makePoint(2.0, 6.0)}; + return std::make_shared(points); +} + +TEST(CatmullRomSubspline, getLength) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline0( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + EXPECT_DOUBLE_EQ(spline0.getLength(), std::hypot(1.0, 3.0)); + + math::geometry::CatmullRomSubspline spline1( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(2.0, 6.0)); + EXPECT_DOUBLE_EQ(spline1.getLength(), std::hypot(1.5, 4.5)); +} + +TEST(CatmullRomSubspline, getLength_zero) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline0(spline_ptr, 0.0, 0.0); + EXPECT_DOUBLE_EQ(spline0.getLength(), 0.0); + + math::geometry::CatmullRomSubspline spline1(spline_ptr, 5.0, 5.0); + EXPECT_DOUBLE_EQ(spline1.getLength(), 0.0); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.0, 0.0), std::hypot(1.5, 4.5)); + + /// @brief Collision at the beginning of the spline + std::vector polygon0{ + makePoint(1.0, 1.0), makePoint(1.0, -1.0), makePoint(-1.0, -1.0), makePoint(-1.0, 1.0)}; + + const auto ans00 = spline.getCollisionPointIn2D(polygon0); + EXPECT_TRUE(ans00); + EXPECT_NEAR(ans00.value(), std::hypot(1.0 / 3.0, 1.0), EPS); + const auto ans01 = spline.getCollisionPointIn2D(polygon0, true); + EXPECT_TRUE(ans01); + EXPECT_NEAR(ans01.value(), std::hypot(1.0 / 3.0, 1.0), EPS); + + /// @brief Collision at the end of the spline + std::vector polygon1{ + makePoint(0.0, 5.0), makePoint(2.0, 5.0), makePoint(2.0, 3.0)}; + + const auto ans10 = spline.getCollisionPointIn2D(polygon1); + EXPECT_TRUE(ans10); + EXPECT_NEAR(ans10.value(), std::hypot(1.25, 3.75), EPS); + const auto ans11 = spline.getCollisionPointIn2D(polygon1, true); + EXPECT_TRUE(ans11); + EXPECT_NEAR(ans11.value(), std::hypot(1.25, 3.75), EPS); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D_shiftedBeginning) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.0, 3.0)); + + /// @brief Collision at the beginning of the spline + std::vector polygon0{ + makePoint(0.0, 3.0), makePoint(0.0, 0.0), makePoint(3.0, 0.0)}; + + const auto ans00 = spline.getCollisionPointIn2D(polygon0); + EXPECT_TRUE(ans00); + EXPECT_NEAR(ans00.value(), std::hypot(0.75 - 0.5, 2.25 - 1.5), EPS); + const auto ans01 = spline.getCollisionPointIn2D(polygon0, true); + EXPECT_TRUE(ans01); + EXPECT_NEAR(ans01.value(), std::hypot(0.75 - 0.5, 2.25 - 1.5), EPS); + + /// @brief Collision at the end of the spline + std::vector polygon1{ + makePoint(2.0, 1.0), makePoint(2.0, 4.0), makePoint(-1.0, 4.0)}; + + const auto ans10 = spline.getCollisionPointIn2D(polygon1); + EXPECT_TRUE(ans10); + EXPECT_NEAR(ans10.value(), std::hypot(0.75 - 0.5, 2.25 - 1.5), EPS); + const auto ans11 = spline.getCollisionPointIn2D(polygon1, true); + EXPECT_TRUE(ans11); + EXPECT_NEAR(ans11.value(), std::hypot(0.75 - 0.5, 2.25 - 1.5), EPS); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D_edge) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + + std::vector polygon{ + makePoint(-2.0, -2.0), makePoint(-2.0, -1.0), makePoint(-1.0, -1.0), makePoint(-1.0, -2.0)}; + + const auto ans0 = spline.getCollisionPointIn2D(polygon); + EXPECT_FALSE(ans0); + const auto ans1 = spline.getCollisionPointIn2D(polygon, true); + EXPECT_FALSE(ans1); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D_base) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + + std::vector polygon{ + makePoint(0.0, 0.0), makePoint(0.0, 1.0), makePoint(1.0, 1.0), makePoint(1.0, 0.0)}; + + const auto ans0 = spline.getCollisionPointIn2D(polygon); + EXPECT_FALSE(ans0); + const auto ans1 = spline.getCollisionPointIn2D(polygon, true); + EXPECT_FALSE(ans1); +} + +TEST(CatmullRomSubspline, getCollisionPointIn2D_wrongPolygon) +{ + const auto spline_ptr = makeLine(); + + math::geometry::CatmullRomSubspline spline( + spline_ptr, std::hypot(0.5, 1.5), std::hypot(1.5, 4.5)); + + std::vector polygon0; + + EXPECT_THROW(spline.getCollisionPointIn2D(polygon0), common::SimulationError); + EXPECT_THROW(spline.getCollisionPointIn2D(polygon0, true), common::SimulationError); + + std::vector polygon1(1); + + EXPECT_THROW(spline.getCollisionPointIn2D(polygon1), common::SimulationError); + EXPECT_THROW(spline.getCollisionPointIn2D(polygon1, true), common::SimulationError); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_collision.cpp b/common/math/geometry/test/test_collision.cpp index eeb6bba08ea..cf60a70eba2 100644 --- a/common/math/geometry/test/test_collision.cpp +++ b/common/math/geometry/test/test_collision.cpp @@ -17,15 +17,13 @@ #include #include +#include "test_utils.hpp" + TEST(Collision, DifferentHeight) { geometry_msgs::msg::Pose pose0; - geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox box; - box.dimensions.x = 1.0; - box.dimensions.y = 1.0; - box.dimensions.z = 1.0; - pose1.position.z = 30.0; + geometry_msgs::msg::Pose pose1 = makePose(0.0, 0.0, 30.0); + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); EXPECT_FALSE(math::geometry::checkCollision2D(pose0, box, pose1, box)); } @@ -33,25 +31,67 @@ TEST(Collision, SamePosition) { geometry_msgs::msg::Pose pose0; geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox box; - box.dimensions.x = 1.0; - box.dimensions.y = 1.0; - box.dimensions.z = 1.0; + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); EXPECT_TRUE(math::geometry::checkCollision2D(pose0, box, pose1, box)); } TEST(Collision, SameHeightNoCollision) { - geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose0 = makePose(0.0, 0.0, 30.0); geometry_msgs::msg::Pose pose1; - traffic_simulator_msgs::msg::BoundingBox box; - box.dimensions.x = 1.0; - box.dimensions.y = 1.0; - box.dimensions.z = 1.0; - pose0.position.x = 30; + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); EXPECT_FALSE(math::geometry::checkCollision2D(pose0, box, pose1, box)); } +TEST(Collision, Touching) +{ + geometry_msgs::msg::Pose pose0; + geometry_msgs::msg::Pose pose1 = makePose(1.0, 1.0, 1.0); + traffic_simulator_msgs::msg::BoundingBox box = makeBbox(1.0, 1.0, 1.0); + EXPECT_TRUE(math::geometry::checkCollision2D(pose0, box, pose1, box)); +} + +TEST(Collision, PointInside) +{ + std::vector polygon(4); + polygon[1] = makePoint(1.0, 0.0); + polygon[2] = makePoint(1.0, 1.0); + polygon[3] = makePoint(0.0, 1.0); + geometry_msgs::msg::Point point = makePoint(0.5, 0.5); + EXPECT_TRUE(math::geometry::contains(polygon, point)); +} + +TEST(Collision, PointOutside) +{ + std::vector polygon(4); + polygon[1] = makePoint(1.0, 0.0); + polygon[2] = makePoint(1.0, 1.0); + polygon[3] = makePoint(0.0, 1.0); + geometry_msgs::msg::Point point = makePoint(1.5, 0.5); + EXPECT_FALSE(math::geometry::contains(polygon, point)); +} + +TEST(Collision, Line) +{ + std::vector polygon(2); + polygon[1] = makePoint(1.0, 1.0); + geometry_msgs::msg::Point point = makePoint(0.5, 0.5); + + bool ans = true; + EXPECT_NO_THROW(ans = math::geometry::contains(polygon, point)); + EXPECT_FALSE(ans); +} + +TEST(Collision, EmptyVector) +{ + std::vector polygon; + geometry_msgs::msg::Point point = makePoint(0.5, 0.5); + + bool ans = true; + EXPECT_NO_THROW(ans = math::geometry::contains(polygon, point)); + EXPECT_FALSE(ans); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/common/math/geometry/test/test_distance.cpp b/common/math/geometry/test/test_distance.cpp index 585bbc18703..05e304dbea8 100644 --- a/common/math/geometry/test/test_distance.cpp +++ b/common/math/geometry/test/test_distance.cpp @@ -18,46 +18,101 @@ #include #include +#include "test_utils.hpp" + TEST(Distance, PointToPoint) { - geometry_msgs::msg::Point p0, p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Point p00, p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Point p10, p11 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Point p20 = makePoint(0.0, 1.0); + geometry_msgs::msg::Point p21 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); } TEST(Distance, PoseToPose) { - geometry_msgs::msg::Pose p0, p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.position.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.position.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Pose p00, p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Pose p10, p11 = makePose(1, 0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Pose p20 = makePose(0.0, 1.0); + geometry_msgs::msg::Pose p21 = makePose(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); } TEST(Distance, PointToPose) { - geometry_msgs::msg::Point p0; - geometry_msgs::msg::Pose p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.position.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Point p00; + geometry_msgs::msg::Pose p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Point p10; + geometry_msgs::msg::Pose p11 = makePose(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Point p20 = makePoint(0.0, 1.0); + geometry_msgs::msg::Pose p21 = makePose(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); } TEST(Distance, PoseToPoint) { - geometry_msgs::msg::Pose p0; - geometry_msgs::msg::Point p1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 0); - p1.x = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), 1); - p0.position.y = 1; - EXPECT_DOUBLE_EQ(math::geometry::getDistance(p0, p1), std::sqrt(2)); + geometry_msgs::msg::Pose p00; + geometry_msgs::msg::Point p01; + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p00, p01), 0.0); + + geometry_msgs::msg::Pose p10; + geometry_msgs::msg::Point p11 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p10, p11), 1.0); + + geometry_msgs::msg::Pose p20 = makePose(0.0, 1.0); + geometry_msgs::msg::Point p21 = makePoint(1.0, 0.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance(p20, p21), std::sqrt(2.0)); +} + +TEST(Distance, Distance2DDisjoint) +{ + std::vector polygon0(4), polygon1(4); + polygon0[0] = makePoint(0.0, 0.0); + polygon0[1] = makePoint(0.0, 1.0); + polygon0[2] = makePoint(1.0, 1.0); + polygon0[3] = makePoint(1.0, 0.0); + polygon1[0] = makePoint(2.0, 2.0); + polygon1[1] = makePoint(2.0, 3.0); + polygon1[2] = makePoint(3.0, 3.0); + polygon1[3] = makePoint(3.0, 2.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance2D(polygon0, polygon1), std::sqrt(2.0)); +} + +TEST(Distance, Distance2DIntersect) +{ + std::vector polygon0(4), polygon1(4); + polygon0[0] = makePoint(0.0, 0.0); + polygon0[1] = makePoint(0.0, 2.0); + polygon0[2] = makePoint(2.0, 2.0); + polygon0[3] = makePoint(2.0, 0.0); + polygon1[0] = makePoint(1.0, 1.0); + polygon1[1] = makePoint(1.0, 3.0); + polygon1[2] = makePoint(3.0, 3.0); + polygon1[3] = makePoint(3.0, 1.0); + EXPECT_DOUBLE_EQ(math::geometry::getDistance2D(polygon0, polygon1), 0.0); +} + +TEST(Distance, Distance2DZeroElements) +{ + std::vector polygon0(4), polygon1; + polygon0[0] = makePoint(0.0, 0.0); + polygon0[1] = makePoint(0.0, 1.0); + polygon0[2] = makePoint(1.0, 1.0); + polygon0[3] = makePoint(1.0, 0.0); + EXPECT_THROW( + math::geometry::getDistance2D(polygon0, polygon1), boost::geometry::empty_input_exception); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_hermite_curve.cpp b/common/math/geometry/test/test_hermite_curve.cpp index f8a3eccfdaf..9efbccef633 100644 --- a/common/math/geometry/test/test_hermite_curve.cpp +++ b/common/math/geometry/test/test_hermite_curve.cpp @@ -16,152 +16,609 @@ #include -TEST(HermiteCurveTest, CheckCollisionToLine) +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-3; + +/// @brief Helper function generating straight line: p(0,0) v(1,0)-> p(1,0) v(1,0) +math::geometry::HermiteCurve makeLine1() { - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - goal_pose.position.x = 1; - start_vec.x = 1; - goal_vec.x = 1; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - EXPECT_DOUBLE_EQ(curve.getLength(), 1); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.1, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.2, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.3, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.4, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.5, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.6, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.7, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.8, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.9, true), 0); - EXPECT_DOUBLE_EQ(curve.get2DCurvature(1.0, true), 0); - EXPECT_DOUBLE_EQ(curve.getPoint(0.5, false).x, 0.5); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x, 1); - EXPECT_DOUBLE_EQ(curve.getMaximum2DCurvature(), 0); - geometry_msgs::msg::Pose p; - p.position.x = 0.1; - p.position.y = 0; - p.position.z = 0; - EXPECT_TRUE(curve.getSValue(p, 1, true)); - EXPECT_TRUE( - (curve.getSValue(p, 1, true).value() > 0.099) && (curve.getSValue(p, 1, true).value() < 0.101)); - { - geometry_msgs::msg::Point start; - start.x = 0.1; - start.y = 1.0; - geometry_msgs::msg::Point goal; - goal.x = 0.1; - goal.y = -1.0; - auto collision_s = curve.getCollisionPointIn2D(start, goal); - EXPECT_FALSE(curve.getCollisionPointIn2D({})); - EXPECT_FALSE(curve.getCollisionPointIn2D({start})); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.1); - } - } - { - geometry_msgs::msg::Point start; - start.x = 0.1; - start.y = 1.0; - geometry_msgs::msg::Point goal; - goal.x = 0.2; - goal.y = -1.0; - auto collision_s = curve.getCollisionPointIn2D(start, goal); - EXPECT_TRUE(collision_s); - if (collision_s) { - EXPECT_DOUBLE_EQ(collision_s.value(), 0.15); - } - } + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); } -TEST(HermiteCurveTest, getNewtonMethodStepSize) {} - -TEST(HermiteCurveTest, CheckNormalVector) -{ - { //p(0,0) v(1,0)-> p(1,1) v(0,1) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 0; - start_pose.position.y = 0; - goal_pose.position.x = 1; - goal_pose.position.y = 1; - start_vec.x = 1; - start_vec.y = 0; - goal_vec.x = 0; - goal_vec.y = 1; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1 / std::sqrt(2)); - } - { //p(0,0) v(1,0)-> p(1,-1) v(0,-1) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 0; - start_pose.position.y = 0; - goal_pose.position.x = 1; - goal_pose.position.y = -1; - start_vec.x = 1; - start_vec.y = 0; - goal_vec.x = 0; - goal_vec.y = -1; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1 / std::sqrt(2)); - } - { //p(1,1) v(0,-1)-> p(0,0) v(-1,0) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 1; - start_pose.position.y = 1; - goal_pose.position.x = 0; - goal_pose.position.y = 0; - start_vec.x = 0; - start_vec.y = -1; - goal_vec.x = -1; - goal_vec.y = 0; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1 / std::sqrt(2)); - } - { //p(1,-1) v(0,1)-> p(0,0) v(-1,0) - geometry_msgs::msg::Pose start_pose, goal_pose; - geometry_msgs::msg::Vector3 start_vec, goal_vec; - start_pose.position.x = 1; - start_pose.position.y = -1; - goal_pose.position.x = 0; - goal_pose.position.y = 0; - start_vec.x = 0; - start_vec.y = 1; - goal_vec.x = -1; - goal_vec.y = 0; - math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); - double norm = std::sqrt( - curve.getTangentVector(0.5, false).x * curve.getTangentVector(0.5, false).x + - curve.getTangentVector(0.5, false).y * curve.getTangentVector(0.5, false).y); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1 / std::sqrt(2)); - EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1 / std::sqrt(2)); +/// @brief Helper function generating straight line: p(0,0) v(1,1)-> p(2,2) v(1,1) +math::geometry::HermiteCurve makeLine2() +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(2.0, 2.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 1.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1.0, 1.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(0,0) v(1,0)-> p(1,1) v(0,1) +math::geometry::HermiteCurve makeCurve1(bool concave_upward = true) +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 1.0); + geometry_msgs::msg::Vector3 start_vec = + concave_upward ? makeVector(1.0, 0.0) : makeVector(0.0, 1.0); + geometry_msgs::msg::Vector3 goal_vec = + concave_upward ? makeVector(0.0, 1.0) : makeVector(1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(0,0) v(1,0)-> p(1,-1) v(0,-1) +math::geometry::HermiteCurve makeCurve2() +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, -1.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(0.0, -1.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(1,1) v(0,-1)-> p(0,0) v(-1,0) +math::geometry::HermiteCurve makeCurve3() +{ + geometry_msgs::msg::Pose start_pose = makePose(1.0, 1.0); + geometry_msgs::msg::Pose goal_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(0.0, -1.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(-1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/// @brief Helper function generating curve: p(1,-1) v(0,1)-> p(0,0) v(-1,0) +math::geometry::HermiteCurve makeCurve4() +{ + geometry_msgs::msg::Pose start_pose = makePose(1.0, -1.0); + geometry_msgs::msg::Pose goal_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(0.0, 1.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(-1.0, 0.0); + return math::geometry::HermiteCurve(start_pose, goal_pose, start_vec, goal_vec); +} + +/** + * @brief Helper function generating a reference trajectory for testing + * @param start_x starting X position + * @param start_y starting Y position + * @param increment_x increment over X axis on every step + * @param increment_y increment over Y axis on every step + * @param vec vector container to generate a reference trajectory in (has to be the size of desired trajectory) + * @param start_idx index from which to start + */ +void generateReferenceTrajectory( + double start_x, double start_y, double increment_x, double increment_y, + std::vector & vec, unsigned int start_idx = 0u) +{ + for (size_t i = 0; i < vec.size(); ++i) { + vec[i].x = start_x + increment_x * (i + start_idx); + vec[i].y = start_y + increment_y * (i + start_idx); } } +TEST(HermiteCurveTest, initializationLine) +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1.0, 0.0); + EXPECT_NO_THROW(math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec)); +} + +TEST(HermiteCurveTest, initializationCurve) +{ + geometry_msgs::msg::Pose start_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(1.0, 1.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(0.0, 1.0); + EXPECT_NO_THROW(math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec)); +} + +TEST(HermiteCurveTest, initializationParams) +{ + EXPECT_NO_THROW(math::geometry::HermiteCurve curve( + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0)); +} + +TEST(HermiteCurveTest, getTrajectoryZero) +{ + const auto curve = makeLine1(); + EXPECT_NO_THROW(auto trajectory = curve.getTrajectory(0)); + auto trajectory = curve.getTrajectory(0); + EXPECT_EQ(trajectory.size(), size_t(0)); +} + +TEST(HermiteCurveTest, getTrajectory) +{ + const auto curve = makeLine1(); + EXPECT_NO_THROW(auto trajectory = curve.getTrajectory(3)); + auto trajectory = curve.getTrajectory(3); + EXPECT_EQ(trajectory.size(), size_t(3)); + EXPECT_POINT_EQ(trajectory[0], makePoint(0.0, 0.0)); + EXPECT_POINT_EQ(trajectory[1], makePoint(0.5, 0.0)); + EXPECT_POINT_EQ(trajectory[2], makePoint(1.0, 0.0)); +} + +TEST(HermiteCurveTest, getTrajectoryReversed) +{ + geometry_msgs::msg::Pose start_pose = makePose(1.0, 0.0); + geometry_msgs::msg::Pose goal_pose = makePose(0.0, 0.0); + geometry_msgs::msg::Vector3 start_vec = makeVector(1.0, 0.0); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1.0, 0.0); + math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); + EXPECT_NO_THROW(auto trajectory = curve.getTrajectory(3)); + auto trajectory = curve.getTrajectory(3); + EXPECT_EQ(trajectory.size(), size_t(3)); + EXPECT_POINT_EQ(trajectory[0], makePoint(1.0, 0.0)); + EXPECT_POINT_EQ(trajectory[1], makePoint(0.5, 0.0)); + EXPECT_POINT_EQ(trajectory[2], makePoint(0.0, 0.0)); +} + +TEST(HermiteCurveTest, getTrajectoryPast1) +{ + const auto curve = makeLine2(); + EXPECT_NO_THROW(auto trajectory = curve.getTrajectory(0.0, 1.0, 0.1, false)); + auto trajectory = curve.getTrajectory(0.0, 1.0, 0.1, false); + std::vector ans(11); + generateReferenceTrajectory(0.0, 0.0, 0.2, 0.2, ans, 1u); + + EXPECT_EQ(trajectory.size(), ans.size()); + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(trajectory[0], ans[0], eps); + EXPECT_POINT_NEAR(trajectory[1], ans[1], eps); + EXPECT_POINT_NEAR(trajectory[2], ans[2], eps); + EXPECT_POINT_NEAR(trajectory[3], ans[3], eps); + EXPECT_POINT_NEAR(trajectory[4], ans[4], eps); + EXPECT_POINT_NEAR(trajectory[5], ans[5], eps); + EXPECT_POINT_NEAR(trajectory[6], ans[6], eps); + EXPECT_POINT_NEAR(trajectory[7], ans[7], eps); + EXPECT_POINT_NEAR(trajectory[8], ans[8], eps); + EXPECT_POINT_NEAR(trajectory[9], ans[9], eps); + EXPECT_POINT_NEAR(trajectory[10], ans[10], eps); +} + +TEST(HermiteCurveTest, getTrajectoryPast2) +{ + const auto curve = makeLine2(); + EXPECT_NO_THROW(auto trajectory = curve.getTrajectory(0.0, std::sqrt(2.0) * 2.0, 0.1, true)); + auto trajectory = curve.getTrajectory(0, std::sqrt(2.0) * 2.0, 0.1 * std::sqrt(2.0) * 2.0, true); + std::vector ans(11); + generateReferenceTrajectory(0.0, 0.0, 0.2, 0.2, ans, 1u); + + EXPECT_EQ(trajectory.size(), ans.size()); + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(trajectory[0], ans[0], eps); + EXPECT_POINT_NEAR(trajectory[1], ans[1], eps); + EXPECT_POINT_NEAR(trajectory[2], ans[2], eps); + EXPECT_POINT_NEAR(trajectory[3], ans[3], eps); + EXPECT_POINT_NEAR(trajectory[4], ans[4], eps); + EXPECT_POINT_NEAR(trajectory[5], ans[5], eps); + EXPECT_POINT_NEAR(trajectory[6], ans[6], eps); + EXPECT_POINT_NEAR(trajectory[7], ans[7], eps); + EXPECT_POINT_NEAR(trajectory[8], ans[8], eps); + EXPECT_POINT_NEAR(trajectory[9], ans[9], eps); + EXPECT_POINT_NEAR(trajectory[10], ans[10], eps); +} + +TEST(HermiteCurveTest, getPointLine) +{ + const auto curve = makeLine2(); + + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(curve.getPoint(0.0, false), makePoint(0.0, 0.0), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.1, false), makePoint(0.2, 0.2), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.2, false), makePoint(0.4, 0.4), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.3, false), makePoint(0.6, 0.6), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.4, false), makePoint(0.8, 0.8), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.5, false), makePoint(1.0, 1.0), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.6, false), makePoint(1.2, 1.2), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.7, false), makePoint(1.4, 1.4), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.8, false), makePoint(1.6, 1.6), eps); + EXPECT_POINT_NEAR(curve.getPoint(0.9, false), makePoint(1.8, 1.8), eps); + EXPECT_POINT_NEAR(curve.getPoint(1.0, false), makePoint(2.0, 2.0), eps); +} + +TEST(HermiteCurveTest, getPointCurve) +{ + geometry_msgs::msg::Pose start_pose = makePose(0, 0); + geometry_msgs::msg::Pose goal_pose = makePose(1, 1); + geometry_msgs::msg::Vector3 start_vec = makeVector(0, 1); + geometry_msgs::msg::Vector3 goal_vec = makeVector(1, 0); + math::geometry::HermiteCurve curve(start_pose, goal_pose, start_vec, goal_vec); + + // The uncertainty of the calculations is high because the parameter space is equidistant + // interpolated, which does not translate into equidistant spline interpolation + constexpr double eps = 0.15; + EXPECT_POINT_NEAR(curve.getPoint(0.0, true), makePoint(0.0, 0.0), eps); + EXPECT_POINT_NEAR( + curve.getPoint(0.75, true), makePoint(1.0 - 1.0 / std::sqrt(2.0), 1.0 / std::sqrt(2.0)), eps); + EXPECT_POINT_NEAR(curve.getPoint(1.5, true), makePoint(1.0, 1.0), eps); +} + +TEST(HermiteCurveTest, get2DCurvatureLine) +{ + const auto curve = makeLine1(); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.0, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.1, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.2, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.3, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.4, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.5, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.6, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.7, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.8, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(0.9, true), 0.0); + EXPECT_DOUBLE_EQ(curve.get2DCurvature(1.0, true), 0.0); +} + +TEST(HermiteCurveTest, get2DCurvatureCurve) +{ + constexpr double eps = 0.01; + const auto curve1 = makeCurve1(); + EXPECT_NEAR(curve1.get2DCurvature(0.0, false), 4.0, eps); + EXPECT_NEAR(curve1.get2DCurvature(0.5, false), 0.45, eps); + EXPECT_NEAR(curve1.get2DCurvature(1.0, false), 4.0, eps); + + const auto curve2 = makeCurve1(false); + EXPECT_NEAR(curve2.get2DCurvature(0.0, false), -4.0, eps); + EXPECT_NEAR(curve2.get2DCurvature(0.5, false), -0.45, eps); + EXPECT_NEAR(curve2.get2DCurvature(1.0, false), -4.0, eps); +} + +TEST(HermiteCurveTest, getMaximum2DCurvatureLine) +{ + const auto curve = makeLine1(); + EXPECT_DOUBLE_EQ(curve.getMaximum2DCurvature(), 0.0); +} + +TEST(HermiteCurveTest, getMaximum2DCurvatureCurve) +{ + constexpr double eps = 0.01; + const auto curve1 = makeCurve1(); + EXPECT_NEAR(curve1.getMaximum2DCurvature(), 4.0, eps); + + const auto curve2 = makeCurve1(false); + EXPECT_NEAR(curve2.getMaximum2DCurvature(), -4.0, eps); +} + +TEST(HermiteCurveTest, getLengthNoParameter) +{ + const auto curve = makeLine1(); + EXPECT_NEAR(curve.getLength(), 1.0, EPS); +} + +TEST(HermiteCurveTest, getLengthParameter) +{ + const auto curve = makeLine1(); + EXPECT_NEAR(curve.getLength(1000), 1.0, EPS); +} + +TEST(HermiteCurveTest, getSValue) +{ + const auto curve = makeLine2(); + + const auto s0 = curve.getSValue(makePose(0.0, 0.0), 1.0, false); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.0, EPS); + const auto s1 = curve.getSValue(makePose(1.0, 1.0), 1.0, false); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.5, EPS); + const auto s2 = curve.getSValue(makePose(2.0, 2.0), 1.0, false); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 1.0, EPS); +} + +TEST(HermiteCurveTest, getSValueAutoscale) +{ + const auto curve = makeLine2(); + + const auto s0 = curve.getSValue(makePose(0.0, 0.0), 1.0, true); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.0, EPS); + const auto s1 = curve.getSValue(makePose(1.0, 1.0), 1.0, true); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.5 * 2.0 * std::sqrt(2.0), EPS); + const auto s2 = curve.getSValue(makePose(2.0, 2.0), 1.0, true); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 2.0 * std::sqrt(2.0), EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceIn2D) +{ + const auto curve = makeLine2(); + + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(2.0, 0.0), 0.5, false), 2.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(2.0, 0.0), 1.0, false), 4.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(-1.0, -2.0), 0.0, false), 5.0, EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceIn2DZeroDistance) +{ + const auto curve = makeLine2(); + + EXPECT_NEAR( + curve.getSquaredDistanceIn2D(makePoint(2.0, 2.0), std::sqrt(2.0) * 2.0, true), 0.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(1.0, 1.0), std::sqrt(2.0), true), 0.0, EPS); + EXPECT_NEAR(curve.getSquaredDistanceIn2D(makePoint(0.0, 0.0), 0.0, true), 0.0, EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceVector) +{ + const auto curve = makeLine2(); + + const auto p0 = curve.getSquaredDistanceVector(makePoint(2.0, 0.0), 0.5, false); + EXPECT_POINT_NEAR(p0, makePoint(1.0, -1.0, 0.0), EPS); + const auto p1 = curve.getSquaredDistanceVector(makePoint(2.0, 0.0), 1.0, false); + EXPECT_POINT_NEAR(p1, makePoint(0.0, -2.0, 0.0), EPS); + const auto p2 = curve.getSquaredDistanceVector(makePoint(-1.0, -2.0), 0.0, false); + EXPECT_POINT_NEAR(p2, makePoint(-1.0, -2.0, 0.0), EPS); +} + +TEST(HermiteCurveTest, getSquaredDistanceVectorZeroDistance) +{ + const auto curve = makeLine2(); + + const auto p0 = curve.getSquaredDistanceVector(makePoint(2.0, 2.0), std::sqrt(2.0) * 2.0, true); + EXPECT_POINT_NEAR(p0, makePoint(0.0, 0.0, 0.0), EPS); + const auto p1 = curve.getSquaredDistanceVector(makePoint(1.0, 1.0), std::sqrt(2.0), true); + EXPECT_POINT_NEAR(p1, makePoint(0.0, 0.0, 0.0), EPS); + const auto p2 = curve.getSquaredDistanceVector(makePoint(0.0, 0.0), 0.0, true); + EXPECT_POINT_NEAR(p2, makePoint(0.0, 0.0, 0.0), EPS); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DLine) +{ + const auto curve = makeLine2(); + + constexpr double eps = 0.1; + const auto s = curve.getCollisionPointIn2D(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + EXPECT_TRUE(s); + EXPECT_NEAR(s.value(), 0.25, eps); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DLineNoCollision) +{ + const auto curve = makeLine2(); + + const auto s = curve.getCollisionPointIn2D(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(s); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DCurve) +{ + const auto curve = makeCurve1(); + + constexpr double eps = 0.1; + const auto s0 = curve.getCollisionPointIn2D(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.5, eps); + + const auto s1 = curve.getCollisionPointIn2D(makePoint(0.1, 0.0), makePoint(1.0, 0.9)); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.2, eps); + + const auto s2 = curve.getCollisionPointIn2D(makePoint(0.1, 0.0), makePoint(1.0, 0.9), true); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 0.8, eps); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DCurveEdge) +{ + const auto curve = makeCurve1(); + + constexpr double eps = 0.1; + const auto s = curve.getCollisionPointIn2D(makePoint(-1.0, 1.0), makePoint(1.0, -1.0)); + EXPECT_TRUE(s); + EXPECT_NEAR(s.value(), 0.0, eps); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DVectorWrongCases) +{ + const auto curve = makeCurve1(); + + std::vector polygon; + EXPECT_FALSE(curve.getCollisionPointIn2D(polygon)); + polygon.emplace_back(makePoint(1.0, 1.0)); + EXPECT_FALSE(curve.getCollisionPointIn2D(polygon)); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DVectorOneCollision) +{ + const auto curve = makeLine1(); + + std::vector polygon{ + makePoint(0.5, 0.5), makePoint(-0.5, 0.5), makePoint(-0.5, -0.5), makePoint(0.5, -0.5)}; + const auto s = curve.getCollisionPointIn2D(polygon); + EXPECT_TRUE(s); + EXPECT_NEAR(s.value(), 0.5, EPS); + EXPECT_FALSE(curve.getCollisionPointIn2D(polygon, false, false)); +} + +TEST(HermiteCurveTest, getCollisionPointIn2DVectorMultipleCollisions) +{ + const auto curve = makeLine2(); + + std::vector polygon{ + makePoint(1, 0), makePoint(2, 1), makePoint(1, 2), makePoint(0, 1)}; + + constexpr double eps = 0.1; + const auto s0 = curve.getCollisionPointIn2D(polygon); + EXPECT_TRUE(s0); + EXPECT_NEAR(s0.value(), 0.25, eps); + + const auto s1 = curve.getCollisionPointIn2D(polygon, true); + EXPECT_TRUE(s1); + EXPECT_NEAR(s1.value(), 0.75, eps); + + const auto s2 = curve.getCollisionPointIn2D(polygon, false, false); + EXPECT_TRUE(s2); + EXPECT_NEAR(s2.value(), 0.75, eps); +} + +TEST(HermiteCurveTest, getTangentVector1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVector2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVector3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVector4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + double norm = + std::hypot(curve.getTangentVector(0.5, false).x, curve.getTangentVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getTangentVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getTangentVectorAutoscale4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getTangentVector(0.75, true).x, curve.getTangentVector(0.75, true).y); + EXPECT_NEAR(curve.getTangentVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getTangentVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVector1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVector2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, 1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVector3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, 1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVector4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + double norm = + std::hypot(curve.getNormalVector(0.5, false).x, curve.getNormalVector(0.5, false).y); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).x / norm, -1.0 / std::sqrt(2.0)); + EXPECT_DOUBLE_EQ(curve.getNormalVector(0.5, false).y / norm, -1.0 / std::sqrt(2.0)); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale1) +{ //p(0,0) v(1,0)-> p(1,1) v(0,1) + const auto curve = makeCurve1(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale2) +{ //p(0,0) v(1,0)-> p(1,-1) v(0,-1) + const auto curve = makeCurve2(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, 1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale3) +{ //p(1,1) v(0,-1)-> p(0,0) v(-1,0) + const auto curve = makeCurve3(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, 1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + +TEST(HermiteCurveTest, getNormalVectorAutoscale4) +{ //p(1,-1) v(0,1)-> p(0,0) v(-1,0) + const auto curve = makeCurve4(); + constexpr double eps = 0.1; + double norm = + std::hypot(curve.getNormalVector(0.75, true).x, curve.getNormalVector(0.75, true).y); + EXPECT_NEAR(curve.getNormalVector(0.75, true).x / norm, -1.0 / std::sqrt(2.0), eps); + EXPECT_NEAR(curve.getNormalVector(0.75, true).y / norm, -1.0 / std::sqrt(2.0), eps); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/common/math/geometry/test/test_intersection.cpp b/common/math/geometry/test/test_intersection.cpp new file mode 100644 index 00000000000..4ee95a08cd9 --- /dev/null +++ b/common/math/geometry/test/test_intersection.cpp @@ -0,0 +1,146 @@ +// 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 "expect_eq_macros.hpp" +#include "test_utils.hpp" + +TEST(Intersection, isIntersect2DDisjoint) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(math::geometry::isIntersect2D(line0, line1)); +} + +TEST(Intersection, isIntersect2DDisjointVector) +{ + std::vector lines{ + {makePoint(0.0, 0.0), makePoint(1.0, 1.0)}, {makePoint(1.0, 0.0), makePoint(2.0, 1.0)}}; + EXPECT_FALSE(math::geometry::isIntersect2D(lines)); +} + +TEST(Intersection, isIntersect2DIntersect) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + EXPECT_TRUE(math::geometry::isIntersect2D(line0, line1)); +} + +TEST(Intersection, isIntersect2DIntersectVector) +{ + std::vector lines{ + {makePoint(1.0, 0.0), makePoint(0.0, 1.0)}, {makePoint(0.0, 0.0), makePoint(1.0, 1.0)}}; + EXPECT_TRUE(math::geometry::isIntersect2D(lines)); +} + +TEST(Intersection, isIntersect2DIdentical) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + EXPECT_TRUE(math::geometry::isIntersect2D(line, line)); +} + +TEST(Intersection, isIntersect2DIdenticalVector) +{ + math::geometry::LineSegment line(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + std::vector lines; + lines.push_back(line); + lines.push_back(line); + lines.push_back(line); + EXPECT_TRUE(math::geometry::isIntersect2D(lines)); +} + +TEST(Intersection, isIntersect2DEmptyVector) +{ + std::vector lines; + bool ans = true; + EXPECT_NO_THROW(ans = math::geometry::isIntersect2D(lines)); + EXPECT_FALSE(ans); +} + +TEST(Intersection, getIntersection2DDisjoint) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(2.0, 1.0)); + EXPECT_FALSE(math::geometry::getIntersection2D(line0, line1)); +} + +TEST(Intersection, getIntersection2DDisjointVector) +{ + std::vector lines{ + {makePoint(0.0, 0.0), makePoint(1.0, 1.0)}, {makePoint(1.0, 0.0), makePoint(2.0, 1.0)}}; + EXPECT_EQ(math::geometry::getIntersection2D(lines).size(), size_t(0)); +} + +TEST(Intersection, getIntersection2DIntersect) +{ + math::geometry::LineSegment line0(makePoint(0.0, 0.0), makePoint(1.0, 1.0)); + math::geometry::LineSegment line1(makePoint(1.0, 0.0), makePoint(0.0, 1.0)); + auto ans = math::geometry::getIntersection2D(line0, line1); + EXPECT_TRUE(ans); + EXPECT_POINT_EQ(ans.value(), makePoint(0.5, 0.5)); +} + +TEST(Intersection, getIntersection2DIntersectVector) +{ + std::vector lines{ + {makePoint(0.0, 0.0), makePoint(1.0, 1.0)}, {makePoint(1.0, 0.0), makePoint(0.0, 1.0)}}; + auto ans = math::geometry::getIntersection2D(lines); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_EQ(ans[0], makePoint(0.5, 0.5)); + EXPECT_POINT_EQ(ans[1], makePoint(0.5, 0.5)); +} + +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()); +} + +TEST(Intersection, getIntersection2DIdenticalVector) +{ + math::geometry::LineSegment line(makePoint(0, 0), makePoint(1, 1)); + std::vector lines; + lines.push_back(line); + 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]); +} + +TEST(Intersection, getIntersection2DEmptyVector) +{ + std::vector lines; + std::vector ans; + EXPECT_NO_THROW(ans = math::geometry::getIntersection2D(lines)); + EXPECT_EQ(ans.size(), size_t(0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp index 5802ad3df9e..021a4f59a36 100644 --- a/common/math/geometry/test/test_line_segment.cpp +++ b/common/math/geometry/test/test_line_segment.cpp @@ -13,15 +13,321 @@ // 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, + quaternion_operation::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, + quaternion_operation::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(LineSegmentTest, GetPoint) +TEST(LineSegment, GetPoint) { { math::geometry::LineSegment line( @@ -235,6 +541,45 @@ TEST(LineSegment, getIntersection2DSValue) // [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. @@ -264,9 +609,14 @@ TEST(LineSegment, getIntersection2DSValue) /// @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] { - EXPECT_TRUE(line.getIntersection2D(math::geometry::LineSegment( + 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)))); + 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 diff --git a/common/math/geometry/test/test_linear_algebra.cpp b/common/math/geometry/test/test_linear_algebra.cpp index d346e9a9498..567f25d3b42 100644 --- a/common/math/geometry/test/test_linear_algebra.cpp +++ b/common/math/geometry/test/test_linear_algebra.cpp @@ -15,64 +15,142 @@ #include #include +#include #include "expect_eq_macros.hpp" +#include "test_utils.hpp" -TEST(LINEAR_ALGEBRA, GET_SIZE) +constexpr double EPS = 1e-3; + +TEST(LINEAR_ALGEBRA, GET_SIZE_ZERO) { geometry_msgs::msg::Vector3 vec; EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), 0.0); - vec.x = 1.0; - vec.y = 0.0; - vec.z = 3.0; +} + +TEST(LINEAR_ALGEBRA, GET_SIZE) +{ + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), std::sqrt(10.0)); } -TEST(LINEAR_ALGEBRA, NORMALIZE) +TEST(LINEAR_ALGEBRA, NORMALIZE_ZERO) { geometry_msgs::msg::Vector3 vec; EXPECT_THROW(math::geometry::normalize(vec), common::SimulationError); - vec.x = 1.0; - vec.y = 0.0; - vec.z = 3.0; +} + +TEST(LINEAR_ALGEBRA, NORMALIZE) +{ + geometry_msgs::msg::Vector3 vec = makeVector(1.0, 0.0, 3.0); vec = math::geometry::normalize(vec); - EXPECT_DOUBLE_EQ(vec.x, 0.31622776601683794); - EXPECT_DOUBLE_EQ(vec.y, 0.0); - EXPECT_DOUBLE_EQ(vec.z, 0.94868329805051377); + + geometry_msgs::msg::Vector3 ans = makeVector(0.31622776601683794, 0.0, 0.94868329805051377); + EXPECT_VECTOR3_EQ(vec, ans); EXPECT_DOUBLE_EQ(math::geometry::getSize(vec), 1.0); } -TEST(LINEAR_ALGEBRA, MULTIPLY) +TEST(LINEAR_ALGEBRA, INNER_PRODUCT) { - geometry_msgs::msg::Vector3 vec = math::geometry::vector3(0, 3, 1); - vec * 1.0; - EXPECT_VECTOR3_EQ((vec * 1.0), math::geometry::vector3(0, 3, 1)); - EXPECT_VECTOR3_EQ((vec * 2.0), math::geometry::vector3(0, 6, 2)); - EXPECT_VECTOR3_EQ((vec * 2.0), (2.0 * vec)); + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0); + EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), -10.0); } -TEST(LINEAR_ALGEBRA, ADDITION) +TEST(LINEAR_ALGEBRA, INNER_PRODUCT_IDENTICAL) { - geometry_msgs::msg::Vector3 vec = math::geometry::vector3(0, 3, 1); - EXPECT_VECTOR3_EQ((vec + vec), (2.0 * vec)); - geometry_msgs::msg::Point p; - p.x = 0; - p.y = 3; - p.z = 1; - EXPECT_VECTOR3_EQ((p + vec), (2.0 * vec)); + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0); + EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec0), 10.0); } -TEST(LINEAR_ALGEBRA, SUBTRACTION) +TEST(LINEAR_ALGEBRA, INNER_PRODUCT_ZERO) { - geometry_msgs::msg::Vector3 vec = math::geometry::vector3(0, 3, 1); - EXPECT_VECTOR3_EQ((vec - vec), geometry_msgs::msg::Vector3()); - geometry_msgs::msg::Point p; - p.x = 0; - p.y = 3; - p.z = 1; + geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0); + EXPECT_DOUBLE_EQ(math::geometry::innerProduct(vec0, vec1), 0.0); +} + +TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0), vec1 = makeVector(-1.0, 0.0, -3.0); + EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec1), M_PI, EPS); +} + +TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE_IDENTICAL) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 0.0, 3.0); + EXPECT_NEAR(math::geometry::getInternalAngle(vec0, vec0), 0.0, EPS); +} + +TEST(LINEAR_ALGEBRA, GET_INTERNAL_ANGLE_ZERO) +{ + geometry_msgs::msg::Vector3 vec0, vec1 = makeVector(1.0, 0.0, 3.0); + EXPECT_THROW(math::geometry::getInternalAngle(vec0, vec1), common::SimulationError); +} + +TEST(LINEAR_ALGEBRA, DIVIDE) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec / 2.0), makeVector(0.0, 1.5, 0.5)); +} + +TEST(LINEAR_ALGEBRA, DIVIDE_ZERO) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + vec = vec / 0.0; + EXPECT_TRUE(std::isnan(vec.x)); + EXPECT_TRUE(std::isinf(vec.y)); + EXPECT_TRUE(std::isinf(vec.z)); +} + +TEST(LINEAR_ALGEBRA, MULTIPLY_FIRST) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec * 2.0), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, MULTIPLY_SECOND) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((2.0 * vec), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, ADDITION_POINT_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + geometry_msgs::msg::Point p = makePoint(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((p + vec), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, ADDITION_VECTOR_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec + vec), makeVector(0.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, ADDITION_POINT_POINT) +{ + geometry_msgs::msg::Point p0 = makePoint(0.0, 3.0, 1.0), p1 = makePoint(2.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((p0 + p1), makePoint(2.0, 6.0, 2.0)); +} + +TEST(LINEAR_ALGEBRA, SUBTRACTION_POINT_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + geometry_msgs::msg::Point p = makePoint(0.0, 3.0, 1.0); EXPECT_VECTOR3_EQ((p - vec), geometry_msgs::msg::Vector3()); } +TEST(LINEAR_ALGEBRA, SUBTRACTION_VECTOR_VECTOR) +{ + geometry_msgs::msg::Vector3 vec = makeVector(0.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((vec - vec), geometry_msgs::msg::Vector3()); +} + +TEST(LINEAR_ALGEBRA, SUBTRACTION_POINT_POINT) +{ + geometry_msgs::msg::Point p0 = makePoint(0.0, 3.0, 1.0), p1 = makePoint(2.0, 3.0, 1.0); + EXPECT_VECTOR3_EQ((p0 - p1), makePoint(-2.0, 0.0, 0.0)); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/common/math/geometry/test/test_polygon.cpp b/common/math/geometry/test/test_polygon.cpp index 0cab8a4535f..8ec0ea97ad9 100644 --- a/common/math/geometry/test/test_polygon.cpp +++ b/common/math/geometry/test/test_polygon.cpp @@ -15,117 +15,116 @@ #include #include +#include #include "expect_eq_macros.hpp" +#include "test_utils.hpp" TEST(Polygon, filterByAxis) +{ + std::vector points{ + makePoint(5.0, 2.0, 3.0), makePoint(1.0, 4.0, 5.0), makePoint(-1.0, 2.0, -3.0)}; + + std::vector values_x = math::geometry::filterByAxis(points, math::geometry::Axis::X); + EXPECT_DOUBLE_EQ(values_x[0], 5.0); + EXPECT_DOUBLE_EQ(values_x[1], 1.0); + EXPECT_DOUBLE_EQ(values_x[2], -1.0); + + std::vector values_y = math::geometry::filterByAxis(points, math::geometry::Axis::Y); + EXPECT_DOUBLE_EQ(values_y[0], 2.0); + EXPECT_DOUBLE_EQ(values_y[1], 4.0); + EXPECT_DOUBLE_EQ(values_y[2], 2.0); + + std::vector values_z = math::geometry::filterByAxis(points, math::geometry::Axis::Z); + EXPECT_DOUBLE_EQ(values_z[0], 3.0); + EXPECT_DOUBLE_EQ(values_z[1], 5.0); + EXPECT_DOUBLE_EQ(values_z[2], -3.0); +} + +TEST(Polygon, filterByAxisEmptyVector) { std::vector points; - geometry_msgs::msg::Point p0; - { - p0.x = 5.0; - p0.y = 2.0; - p0.z = 3.0; - } - points.emplace_back(p0); - geometry_msgs::msg::Point p1; - { - p1.x = 1.0; - p1.y = 4.0; - p1.z = 5.0; - } - points.emplace_back(p1); - geometry_msgs::msg::Point p2; - { - p2.x = -1.0; - p2.y = 2.0; - p2.z = -3.0; - } - points.emplace_back(p2); - std::vector values; - values = math::geometry::filterByAxis(points, math::geometry::Axis::X); - EXPECT_DOUBLE_EQ(values[0], 5); - EXPECT_DOUBLE_EQ(values[1], 1); - EXPECT_DOUBLE_EQ(values[2], -1); - values = math::geometry::filterByAxis(points, math::geometry::Axis::Y); - EXPECT_DOUBLE_EQ(values[0], 2); - EXPECT_DOUBLE_EQ(values[1], 4); - EXPECT_DOUBLE_EQ(values[2], 2); - values = math::geometry::filterByAxis(points, math::geometry::Axis::Z); - EXPECT_DOUBLE_EQ(values[0], 3); - EXPECT_DOUBLE_EQ(values[1], 5); - EXPECT_DOUBLE_EQ(values[2], -3); + + std::vector values_x = math::geometry::filterByAxis(points, math::geometry::Axis::X); + EXPECT_EQ(values_x.size(), size_t(0)); + + std::vector values_y = math::geometry::filterByAxis(points, math::geometry::Axis::Y); + EXPECT_EQ(values_y.size(), size_t(0)); + + std::vector values_z = math::geometry::filterByAxis(points, math::geometry::Axis::Z); + EXPECT_EQ(values_z.size(), size_t(0)); +} + +TEST(Polygon, getMaxValue) +{ + std::vector points{ + makePoint(5.0, 2.0, 3.0), makePoint(1.0, 4.0, 5.0), makePoint(-1.0, 2.0, -3.0)}; + EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::X), 5.0); + EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Y), 4.0); + EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Z), 5.0); } -TEST(Polygon, GetMinMaxValue) +TEST(Polygon, getMaxValueEmptyVector) { std::vector points; - geometry_msgs::msg::Point p0; - { - p0.x = 5.0; - p0.y = 2.0; - p0.z = 3.0; - } - points.emplace_back(p0); - geometry_msgs::msg::Point p1; - { - p1.x = 1.0; - p1.y = 4.0; - p1.z = 5.0; - } - points.emplace_back(p1); - geometry_msgs::msg::Point p2; - { - p2.x = -1.0; - p2.y = 2.0; - p2.z = -3.0; - } - points.emplace_back(p2); - EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::X), 5); - EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::X), -1); - EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Y), 4); - EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Y), 2); - EXPECT_DOUBLE_EQ(math::geometry::getMaxValue(points, math::geometry::Axis::Z), 5); - EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Z), -3); + EXPECT_THROW( + math::geometry::getMaxValue(points, math::geometry::Axis::X), common::SimulationError); + EXPECT_THROW( + math::geometry::getMaxValue(points, math::geometry::Axis::Y), common::SimulationError); + EXPECT_THROW( + math::geometry::getMaxValue(points, math::geometry::Axis::Z), common::SimulationError); } -TEST(Polygon, get2DConvexHull) +TEST(Polygon, getMinValue) +{ + std::vector points{ + makePoint(5.0, 2.0, 3.0), makePoint(1.0, 4.0, 5.0), makePoint(-1.0, 2.0, -3.0)}; + EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::X), -1.0); + EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Y), 2.0); + EXPECT_DOUBLE_EQ(math::geometry::getMinValue(points, math::geometry::Axis::Z), -3.0); +} + +TEST(Polygon, getMinValueEmptyVector) { std::vector points; - geometry_msgs::msg::Point p0; - { - p0.x = 2.0; - p0.y = 2.0; - p0.z = 0.0; - } - points.emplace_back(p0); - geometry_msgs::msg::Point p1; - { - p1.x = -2.0; - p1.y = 2.0; - p1.z = 0.0; - } - points.emplace_back(p1); - geometry_msgs::msg::Point p2; - { - p2.x = -2.0; - p2.y = -2.0; - p2.z = 0.0; - } - points.emplace_back(p2); - geometry_msgs::msg::Point p3; - { - p3.x = -1.0; - p3.y = 0.0; - p3.z = 0.0; - } - points.emplace_back(p3); + EXPECT_THROW( + math::geometry::getMinValue(points, math::geometry::Axis::X), common::SimulationError); + EXPECT_THROW( + math::geometry::getMinValue(points, math::geometry::Axis::Y), common::SimulationError); + EXPECT_THROW( + math::geometry::getMinValue(points, math::geometry::Axis::Z), common::SimulationError); +} + +TEST(Polygon, get2DConvexHull) +{ + std::vector points{ + makePoint(2.0, 2.0), makePoint(-2.0, 2.0), makePoint(-2.0, -2.0), makePoint(-1.0, 0.0)}; const auto hull = math::geometry::get2DConvexHull(points); EXPECT_EQ(hull.size(), static_cast(4)); - EXPECT_POINT_EQ(hull[0], p2); - EXPECT_POINT_EQ(hull[1], p1); - EXPECT_POINT_EQ(hull[2], p0); - EXPECT_POINT_EQ(hull[3], p2); + EXPECT_POINT_EQ(hull[0], points[2]); + EXPECT_POINT_EQ(hull[1], points[1]); + EXPECT_POINT_EQ(hull[2], points[0]); + EXPECT_POINT_EQ(hull[3], points[2]); +} + +TEST(Polygon, get2DConvexHullIdle) +{ + std::vector points{ + makePoint(2.0, 2.0), makePoint(-2.0, 2.0), makePoint(-2.0, -2.0), makePoint(2.0, -2.0)}; + const auto hull = math::geometry::get2DConvexHull(points); + EXPECT_EQ(hull.size(), static_cast(5)); + EXPECT_POINT_EQ(hull[0], points[2]); + EXPECT_POINT_EQ(hull[1], points[1]); + EXPECT_POINT_EQ(hull[2], points[0]); + EXPECT_POINT_EQ(hull[3], points[3]); + EXPECT_POINT_EQ(hull[4], points[2]); +} + +TEST(Polygon, get2DConvexHullEmpty) +{ + std::vector points; + const auto hull = math::geometry::get2DConvexHull(points); + EXPECT_EQ(hull.size(), size_t(0)); } int main(int argc, char ** argv) diff --git a/common/math/geometry/test/test_transform.cpp b/common/math/geometry/test/test_transform.cpp new file mode 100644 index 00000000000..f544d02f4cc --- /dev/null +++ b/common/math/geometry/test/test_transform.cpp @@ -0,0 +1,169 @@ +// 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 "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-3; + +geometry_msgs::msg::Pose getFilledPose() +{ + geometry_msgs::msg::Pose pose; + pose.position = makePoint(1.0, 2.0, 3.0); + pose.orientation = + quaternion_operation::convertEulerAngleToQuaternion(makeVector(90.0 * M_PI / 180.0, 0.0)); + return pose; +} + +TEST(Transform, getRelativePoseDifferent) +{ + geometry_msgs::msg::Pose pose0 = getFilledPose(); + geometry_msgs::msg::Pose pose1 = makePose(3.0, 0.0); + + geometry_msgs::msg::Pose ans = makePose( + 2.0, -3.0, 2.0, + quaternion_operation::convertEulerAngleToQuaternion(makeVector(-90.0 * M_PI / 180.0, 0.0))); + EXPECT_POSE_NEAR(math::geometry::getRelativePose(pose0, pose1), ans, EPS); +} + +TEST(Transform, getRelativePoseIdentical) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + EXPECT_POSE_EQ(math::geometry::getRelativePose(pose, pose), geometry_msgs::msg::Pose()); +} + +TEST(Transform, transformPointRealPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + + geometry_msgs::msg::Point ans = makePoint(2.0, -1.0, 5.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, point), ans); +} + +TEST(Transform, transformPointNoPose) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, point), point); +} + +TEST(Transform, transformPointRealSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose = getFilledPose(); + sensor_pose.orientation = geometry_msgs::msg::Quaternion(); + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + + geometry_msgs::msg::Point ans = makePoint(1.0, -3.0, 2.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, sensor_pose, point), ans); +} + +TEST(Transform, transformPointNoSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose; + geometry_msgs::msg::Point point = makePoint(1.0, 2.0, 3.0); + + geometry_msgs::msg::Point ans = makePoint(2.0, -1.0, 5.0); + EXPECT_POINT_EQ(math::geometry::transformPoint(pose, sensor_pose, point), ans); +} + +TEST(Transform, transformPointsRealPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + geometry_msgs::msg::Point ans0 = makePoint(2.0, -1.0, 5.0); + geometry_msgs::msg::Point ans1 = makePoint(2.0, -1.0, 7.0); + + std::vector ans = math::geometry::transformPoints(pose, points); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], ans0, EPS); + EXPECT_POINT_NEAR(ans[1], ans1, EPS); +} + +TEST(Transform, transformPointsNoPose) +{ + geometry_msgs::msg::Pose pose; + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + std::vector ans = math::geometry::transformPoints(pose, points); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], points[0], EPS); + EXPECT_POINT_NEAR(ans[1], points[1], EPS); +} + +TEST(Transform, transformPointsEmptyVector) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + std::vector points; + + std::vector ans; + EXPECT_NO_THROW(ans = math::geometry::transformPoints(pose, points)); + EXPECT_EQ(ans.size(), size_t(0)); +} + +TEST(Transform, transformPointsRealSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose = getFilledPose(); + sensor_pose.orientation = geometry_msgs::msg::Quaternion(); + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + std::vector ans = + math::geometry::transformPoints(pose, sensor_pose, points); + geometry_msgs::msg::Point ans0 = makePoint(1.0, -3.0, 2.0); + geometry_msgs::msg::Point ans1 = makePoint(1.0, -3.0, 4.0); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], ans0, EPS); + EXPECT_POINT_NEAR(ans[1], ans1, EPS); +} + +TEST(Transform, transformPointsNoSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose; + std::vector points{makePoint(1.0, 2.0, 3.0), makePoint(1.0, 4.0, 3.0)}; + + std::vector ans = + math::geometry::transformPoints(pose, sensor_pose, points); + geometry_msgs::msg::Point ans0 = makePoint(2.0, -1.0, 5.0); + geometry_msgs::msg::Point ans1 = makePoint(2.0, -1.0, 7.0); + EXPECT_EQ(ans.size(), size_t(2)); + EXPECT_POINT_NEAR(ans[0], ans0, EPS); + EXPECT_POINT_NEAR(ans[1], ans1, EPS); +} + +TEST(Transform, transformPointsEmptyVectorWithSensorPose) +{ + geometry_msgs::msg::Pose pose = getFilledPose(); + geometry_msgs::msg::Pose sensor_pose = getFilledPose(); + std::vector points; + + std::vector ans; + EXPECT_NO_THROW(ans = math::geometry::transformPoints(pose, sensor_pose, points)); + EXPECT_EQ(ans.size(), size_t(0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/math/geometry/test/test_utils.hpp b/common/math/geometry/test/test_utils.hpp new file mode 100644 index 00000000000..31e99e3bdb4 --- /dev/null +++ b/common/math/geometry/test/test_utils.hpp @@ -0,0 +1,107 @@ +// 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 GEOMETRY__TEST__TEST_UTILS_HPP_ +#define GEOMETRY__TEST__TEST_UTILS_HPP_ + +#include +#include +#include +#include + +/** + * Creates a `geometry_msgs::msg::Point` object with the specified coordinates. + * + * @param x The x-coordinate of the point. + * @param y The y-coordinate of the point. + * @param z The z-coordinate of the point. Defaults to 0 if not provided. + * + * @return The created `geometry_msgs::msg::Point` object with the specified coordinates. + */ +inline geometry_msgs::msg::Point makePoint(double x, double y, double z = 0.0) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +/** + * Creates a `geometry_msgs::msg::Vector3` object with the specified values for `x`, `y`, and `z` coordinates. + * + * @param x The value for the `x` coordinate. + * @param y The value for the `y` coordinate. + * @param z The value for the `z` coordinate. Default value is 0. + * + * @return A `geometry_msgs::msg::Vector3` object with the specified values for `x`, `y`, and `z` coordinates. + */ +inline geometry_msgs::msg::Vector3 makeVector(double x, double y, double z = 0.0) +{ + geometry_msgs::msg::Vector3 v; + v.x = x; + v.y = y; + v.z = z; + return v; +} + +/** + * Generates a `Pose` message with the given coordinates and orientation. + * + * @param x The x-coordinate of the position. + * @param y The y-coordinate of the position. + * @param z The z-coordinate of the position (default: 0). + * @param q The orientation of the pose (default: identity quaternion). + * + * @return A `Pose` message with the specified coordinates and orientation. + */ +inline geometry_msgs::msg::Pose makePose( + double x, double y, double z = 0.0, + geometry_msgs::msg::Quaternion q = geometry_msgs::msg::Quaternion()) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = y; + p.position.z = z; + p.orientation = q; + return p; +} + +/** + * Generates a bounding box with the given dimensions and center coordinates. + * + * @param dim_x The dimension of the bounding box along the x-axis. + * @param dim_y The dimension of the bounding box along the y-axis. + * @param dim_z The dimension of the bounding box along the z-axis. (default = 0.0) + * @param center_x The x-coordinate of the center of the bounding box. (default = 0.0) + * @param center_y The y-coordinate of the center of the bounding box. (default = 0.0) + * @param center_z The z-coordinate of the center of the bounding box. (default = 0.0) + * + * @return The generated bounding box. + */ +inline traffic_simulator_msgs::msg::BoundingBox makeBbox( + double dim_x, double dim_y, double dim_z = 0.0, double center_x = 0.0, double center_y = 0.0, + double center_z = 0.0) +{ + traffic_simulator_msgs::msg::BoundingBox bbox; + bbox.dimensions.x = dim_x; + bbox.dimensions.y = dim_y; + bbox.dimensions.z = dim_z; + bbox.center.x = center_x; + bbox.center.y = center_y; + bbox.center.z = center_z; + return bbox; +} + +#endif // GEOMETRY__TEST__TEST_UTILS_HPP_ diff --git a/common/math/geometry/test/vector3/CMakeLists.txt b/common/math/geometry/test/vector3/CMakeLists.txt new file mode 100644 index 00000000000..4c258e2d872 --- /dev/null +++ b/common/math/geometry/test/vector3/CMakeLists.txt @@ -0,0 +1,6 @@ +# These tests have to be separate, because one requires including operator.hpp and other requires not including operator.hpp +ament_add_gtest(test_truncate + test_vector3.cpp + test_truncate_msg.cpp + test_truncate_custom.cpp) +target_link_libraries(test_truncate geometry) diff --git a/common/math/geometry/test/vector3/test_truncate_custom.cpp b/common/math/geometry/test/vector3/test_truncate_custom.cpp new file mode 100644 index 00000000000..0ded3633562 --- /dev/null +++ b/common/math/geometry/test/vector3/test_truncate_custom.cpp @@ -0,0 +1,56 @@ +// 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 "../expect_eq_macros.hpp" + +constexpr double EPS = 1e-6; + +/** + * @brief Custom Vector3 struct using T type with multiplication and division operators + */ +template +struct CustomVector3 +{ + T x, y, z; + CustomVector3() = default; + CustomVector3(T x, T y, T z) : x(x), y(y), z(z) {} + template + CustomVector3 operator*(U v) const + { + return CustomVector3(x * v, y * v, z * v); + } + template + CustomVector3 operator/(U v) const + { + return CustomVector3(x / v, y / v, z / v); + } +}; + +TEST(Vector3, truncate_customVectorBelowMax) +{ + CustomVector3 vec0(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, 16), vec0) +} + +TEST(Vector3, truncate_customVectorOverMax) +{ + CustomVector3 vec0(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, std::sqrt(12.0)), CustomVector3(2.0, 2.0, 2.0)); +} diff --git a/common/math/geometry/test/vector3/test_truncate_msg.cpp b/common/math/geometry/test/vector3/test_truncate_msg.cpp new file mode 100644 index 00000000000..454007e1cf5 --- /dev/null +++ b/common/math/geometry/test/vector3/test_truncate_msg.cpp @@ -0,0 +1,37 @@ +// 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 // this header is neaded for truncate to work with geometry_msgs::msg::Vector3 +#include + +#include "../expect_eq_macros.hpp" +#include "../test_utils.hpp" + +constexpr double EPS = 1e-6; + +TEST(Vector3, truncate_msgVectorBelowMax) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, 16), vec0) +} + +TEST(Vector3, truncate_msgVectorOverMax) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(4.0, 4.0, 4.0); + EXPECT_VECTOR3_EQ(math::geometry::truncate(vec0, std::sqrt(12.0)), makeVector(2.0, 2.0, 2.0)); +} diff --git a/common/math/geometry/test/vector3/test_vector3.cpp b/common/math/geometry/test/vector3/test_vector3.cpp new file mode 100644 index 00000000000..6f47513d8a0 --- /dev/null +++ b/common/math/geometry/test/vector3/test_vector3.cpp @@ -0,0 +1,190 @@ +// 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 "../expect_eq_macros.hpp" +#include "../test_utils.hpp" + +constexpr double EPS = 1e-6; + +/** + * @brief Custom Vector3 struct using T type + */ +template +struct CustomVector3 +{ + T x, y, z; + CustomVector3() = default; + CustomVector3(T x, T y, T z) : x(x), y(y), z(z) {} +}; + +TEST(Vector3, hypot_msgVector) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_DOUBLE_EQ(math::geometry::hypot(vec0, vec1), 6.0); +} + +TEST(Vector3, hypot_customVector) +{ + CustomVector3 vec0(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_DOUBLE_EQ(math::geometry::hypot(vec0, vec1), 6.0); +} + +TEST(Vector3, norm_msgVector) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + + EXPECT_DOUBLE_EQ(math::geometry::norm(vec0), std::sqrt(14.0)); +} + +TEST(Vector3, norm_customVector) +{ + CustomVector3 vec0(1u, 2u, 3u); + + EXPECT_DOUBLE_EQ(math::geometry::norm(vec0), std::sqrt(14.0)); +} + +TEST(Vector3, normalize_msgVector) +{ + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + + const double norm = std::sqrt(14.0); + EXPECT_VECTOR3_EQ( + math::geometry::normalize(vec0), makeVector(1.0 / norm, 2.0 / norm, 3.0 / norm)); +} + +TEST(Vector3, normalize_customVector) +{ + CustomVector3 vec0(1.0, 2.0, 3.0); + + const double norm = std::sqrt(14.0); + EXPECT_VECTOR3_NEAR( + math::geometry::normalize(vec0), makeVector(1.0 / norm, 2.0 / norm, 3.0 / norm), EPS); +} + +TEST(Vector3, addition_msgVector) +{ + using math::geometry::operator+; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 + vec1), makeVector(0.0, 0.0, 2.0)); +} + +TEST(Vector3, addition_customVector) +{ + using math::geometry::operator+; + + CustomVector3 vec0(1u, 2u, 3u); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 + vec1), makeVector(0.0, 0.0, 2.0)); +} + +TEST(Vector3, subtraction_msgVector) +{ + using math::geometry::operator-; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 - vec1), makeVector(2.0, 4.0, 4.0)); +} + +TEST(Vector3, subtraction_CustomVector) +{ + using math::geometry::operator-; + + CustomVector3 vec0(1u, 2u, 3u); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + + EXPECT_VECTOR3_EQ((vec0 - vec1), makeVector(2.0, 4.0, 4.0)); +} + +TEST(Vector3, multiplication_msgVector) +{ + using math::geometry::operator*; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + + EXPECT_VECTOR3_EQ((vec0 * 3.0), makeVector(3.0, 6.0, 9.0)); +} + +TEST(Vector3, multiplication_CustomVector) +{ + using math::geometry::operator*; + + CustomVector3 vec0(1u, 2u, 3u); + + EXPECT_VECTOR3_EQ((vec0 * 3.0), makeVector(3.0, 6.0, 9.0)); +} + +TEST(Vector3, division_msgVector) +{ + using math::geometry::operator/; + + geometry_msgs::msg::Vector3 vec0 = makeVector(2.0, 14.0, 6.0); + + EXPECT_VECTOR3_EQ((vec0 / 2.0), makeVector(1.0, 7.0, 3.0)); +} + +TEST(Vector3, division_CustomVector) +{ + using math::geometry::operator/; + + CustomVector3 vec0(2u, 14u, 6u); + + EXPECT_VECTOR3_EQ((vec0 / 2.0), makeVector(1.0, 7.0, 3.0)); +} + +TEST(Vector3, additionAssignment_msgVector) +{ + using math::geometry::operator+=; + + geometry_msgs::msg::Vector3 vec0 = makeVector(1.0, 2.0, 3.0); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + vec0 += vec1; + + EXPECT_VECTOR3_EQ(vec0, makeVector(0.0, 0.0, 2.0)); +} + +TEST(Vector3, additionAssignment_customVector) +{ + using math::geometry::operator+=; + + CustomVector3 vec0(1u, 2u, 3u); + geometry_msgs::msg::Vector3 vec1 = makeVector(-1.0, -2.0, -1.0); + vec0 += vec1; + + EXPECT_VECTOR3_EQ(vec0, makeVector(0.0, 0.0, 2.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 91125859403..38f87d3c966 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package scenario_simulator_exception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#1113 `_ from tier4/feature/doxygen +* add define error category +* add SpecificationViolation +* update Doxyfile +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* ref(openscenario_interpreter): revert comments format +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* feat(openscenario_interpreter): inherit ScenarioFailure and tidy up +* feat(openscenario_interpreter): mv ScenarioError, add catch InitActions +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* feat(openscenario_interpreter): improve exception for detail result msg +* feat(openscenario_interpreter): first step detail result msg +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 8d77216275b..d8941a7e9d7 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 0.8.0 + 1.0.0 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 5221c2efed8..5d95865d350 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -2,6 +2,43 @@ Changelog for package junit_exporter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#1113 `_ from tier4/feature/doxygen +* fix typo +* update Doxyfile +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 2f07b166c8e..f82211014d7 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 0.8.0 + 1.0.0 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 1bc3d9cab6f..921bcf4ad60 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package status_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge pull request `#1077 `_ from tier4/fix/autoware-shutdown + Fix/autoware shutdown +* Add new member function `StatusMonitor::overrideThreshold` +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Masaya Kataoka, Paweł Lech, Tatsuya Yamasaki, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/common/status_monitor/include/status_monitor/status_monitor.hpp b/common/status_monitor/include/status_monitor/status_monitor.hpp index 3b8d083d542..e11373d82df 100644 --- a/common/status_monitor/include/status_monitor/status_monitor.hpp +++ b/common/status_monitor/include/status_monitor/status_monitor.hpp @@ -105,6 +105,38 @@ class StatusMonitor } } + template + class ScopedExchanger + { + std::reference_wrapper target; + + T value; + + public: + template + static auto locked_exchange(Ts &&... xs) -> decltype(auto) + { + auto lock = std::scoped_lock(mutex); + return std::exchange(std::forward(xs)...); + } + + template + explicit ScopedExchanger(T & x, U && y) + : target(x), value(locked_exchange(target.get(), std::forward(y))) + { + } + + ~ScopedExchanger() { locked_exchange(target.get(), value); } + }; + + template + auto overrideThreshold(const std::chrono::seconds & t, Thunk thunk) -> decltype(auto) + { + auto exchanger = ScopedExchanger(threshold, t); + + return thunk(); + } + auto write() const -> void; } static status_monitor; } // namespace common diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 72cdb9598da..ac48f79e7bc 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 0.8.0 + 1.0.0 none Tatsuya Yamasaki Apache License 2.0 diff --git a/common/status_monitor/src/status_monitor.cpp b/common/status_monitor/src/status_monitor.cpp index da5879ce2f5..b00c8395bad 100644 --- a/common/status_monitor/src/status_monitor.cpp +++ b/common/status_monitor/src/status_monitor.cpp @@ -120,9 +120,11 @@ auto StatusMonitor::write() const -> void } } + json["good"] = good(); + json["name"] = name(); - json["good"] = good(); + json["threshold"] = threshold.count(); json["unix_time"] = std::chrono::duration_cast( std::chrono::high_resolution_clock::now().time_since_epoch()) diff --git a/docs/ReleaseNotes.md b/docs/ReleaseNotes.md index 6090708298b..225831750b5 100644 --- a/docs/ReleaseNotes.md +++ b/docs/ReleaseNotes.md @@ -1,33 +1,58 @@ # Release Notes +!!! Warning + This release note does not record any update differences since version 1.0.0. + If you would like to check for update differences since version 1.0.0, please refer to [this page.](https://github.com/tier4/scenario_simulator_v2/releases) + ## Difference between the latest release and master Major Changes :race_car: :red_car: :blue_car: -| Feature | Brief summary | Category | Pull request | Contributor | -| --------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------- | ----------------------------------------------------------------- | --------------------------------------------- | -| Traffic Light Simulation | Start supporting to traffic light messages in `autoware_perception_msgs`. | `openscenario_interpreter`, `traffic_simulator`, `simple_sensor_simulator` | [#1027](https://github.com/tier4/scenario_simulator_v2/pull/1027) | [HansRobo](https://github.com/HansRobo) | -| Detailed control of permissions for autonomous decisions | Add `featureIdentifiersRequiringExternalPermissionForAutonomousDecisions` property for ObjectController. scenario_simulator_v2 controls auto_mode of Autoware planning modules by settings in the property. | `openscenario_interpreter`, `concealer` | [#1092](https://github.com/tier4/scenario_simulator_v2/pull/1092) | [HansRobo](https://github.com/HansRobo) | -| OpenSCENARIO `Controller.Properties.Property` | Support the property `allowGoalModification` to turn on the Autoware feature `allow_goal_modification`. | `openscenario_interpreter`, `concealer` | [#997](https://github.com/tier4/scenario_simulator_v2/pull/997) | [yamacir-kit](https://github.com/yamacir-kit) | -| OpenSCENARIO 1.2 `UserDefinedAction.CustomCommandAction` | Add `PseudoTrafficSignalDetectorConfidenceSetAction@v1`, a CustomCommandAction to control confidences in traffic light topic. | `openscenario_interpreter`, `traffic_simulator` | [#1111](https://github.com/tier4/scenario_simulator_v2/pull/1111) | [HansRobo](https://github.com/HansRobo) | -| Ego Vehicle Simulation | Update ego vehicle models ( Import `simple_planning_simulator` changes except [autowarefoundation/autoware.universe#4941](https://github.com/autowarefoundation/autoware.universe/pull/4941) ) | `simple_sensor_simulator` | [#1144](https://github.com/tier4/scenario_simulator_v2/pull/1144) | [HansRobo](https://github.com/HansRobo) | -| Add `API::getLaneletLength()` and `API::getMapPoseFromRelativePose()` | Add `API::getLaneletLength()`to get a length of target lanelet, and `API::getMapPoseFromRelativePose()` to transform relative pose to map pose. | `traffic_simulator` | [#1145](https://github.com/tier4/scenario_simulator_v2/pull/1145) | [hakuturu583](https://github.com/hakuturu583) | -| Start supporting ego entity in C++ scenario | Enable spawn ego entity in C++ scenarios in cpp_mock_scenarios package. | `cpp_mock_scenarios` | [#1145](https://github.com/tier4/scenario_simulator_v2/pull/1145) | [hakuturu583](https://github.com/hakuturu583) | - +| Feature | Brief summary | Category | Pull request | Contributor | +| ----------------------- | -------------------------------------------------------------------- | ------------------- | ----------------------------------------------------------------- | --------------------------------------------- | +| Extend matching length. | Enable consider tread when calculating matching length of EgoEntity. | `traffic_simulator` | [#1181](https://github.com/tier4/scenario_simulator_v2/pull/1181) | [hakuturu583](https://github.com/hakuturu583) | Bug Fixes:bug: -| Feature | Brief summary | Category | Pull request | Contributor | -| ---------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------- | ----------------------------------------------------------------- | --------------------------------------------- | -| Deleted entity management | Fix crash caused by `DeleteEntityAction` by handling deleted entity explicitly. | `traffic_simulator` | [#1096](https://github.com/tier4/scenario_simulator_v2/pull/1096) | [f0reachARR](https://github.com/f0reachARR) | -| OpenSCENARIO `RelativeDistanceCondition` | Restore the behavior of the existing distance measurement mode of RelativeDistanceCondition that was changed in #1095. | `openscenario_interpreter` | [#1121](https://github.com/tier4/scenario_simulator_v2/pull/1121) | [yamacir-kit](https://github.com/yamacir-kit) | -| Suppression of duplicate node names | Fix an issue where nodes with the same name were being created. (especially with temporary nodes for `UserDefinedAction.CustomCommandAction`) | `scenario_test_runner`, `openscenario_interpreter` | [#1126](https://github.com/tier4/scenario_simulator_v2/pull/1126) | [lchojnack](https://github.com/lchojnack) | +| Feature | Brief summary | Category | Pull request | Contributor | +| ------------------------------ | -------------------------------------------------------------- | ------------------- | ----------------------------------------------------------------- | --------------------------------------------- | +| Revert #1096 | Revert incomplete fix for de-spawned entity. | `traffic_simulator` | [#1159](https://github.com/tier4/scenario_simulator_v2/pull/1159) | [HansRobo](https://github.com/HansRobo) | +| Fix problems in route planning | Fixed a bug in a function related to Entity's route selection. | `traffic_simulator` | [#1126](https://github.com/tier4/scenario_simulator_v2/pull/1168) | [hakuturu583](https://github.com/hakuturu583) | Minor Tweaks :oncoming_police_car: | Feature | Brief summary | Category | Pull request | Contributor | | ------- | ------------- | -------- | ------------ | ----------- | -| | | | | | + +## Version 0.9.0 + +Major Changes :race_car: :red_car: :blue_car: + +| Feature | Brief summary | Category | Pull request | Contributor | +| --------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------- | ----------------------------------------------------------------- | ------------------------------------------------------- | +| Traffic Light Simulation | Start supporting to traffic light messages in `autoware_perception_msgs`. | `openscenario_interpreter`, `traffic_simulator`, `simple_sensor_simulator` | [#1027](https://github.com/tier4/scenario_simulator_v2/pull/1027) | [HansRobo](https://github.com/HansRobo) | +| Detailed control of permissions for autonomous decisions | Add `featureIdentifiersRequiringExternalPermissionForAutonomousDecisions` property for ObjectController. scenario_simulator_v2 controls auto_mode of Autoware planning modules by settings in the property. | `openscenario_interpreter`, `concealer` | [#1092](https://github.com/tier4/scenario_simulator_v2/pull/1092) | [HansRobo](https://github.com/HansRobo) | +| OpenSCENARIO `Controller.Properties.Property` | Support the property `allowGoalModification` to turn on the Autoware feature `allow_goal_modification`. | `openscenario_interpreter`, `concealer` | [#997](https://github.com/tier4/scenario_simulator_v2/pull/997) | [yamacir-kit](https://github.com/yamacir-kit) | +| OpenSCENARIO 1.2 `UserDefinedAction.CustomCommandAction` | Add `PseudoTrafficSignalDetectorConfidenceSetAction@v1`, a CustomCommandAction to control confidences in traffic light topic. | `openscenario_interpreter`, `traffic_simulator` | [#1111](https://github.com/tier4/scenario_simulator_v2/pull/1111) | [HansRobo](https://github.com/HansRobo) | +| Ego Vehicle Simulation | Update ego vehicle models ( Import `simple_planning_simulator` changes except [autowarefoundation/autoware.universe#4941](https://github.com/autowarefoundation/autoware.universe/pull/4941) ) | `simple_sensor_simulator` | [#1144](https://github.com/tier4/scenario_simulator_v2/pull/1144) | [HansRobo](https://github.com/HansRobo) | +| Add `API::getLaneletLength()` and `API::getMapPoseFromRelativePose()` | Add `API::getLaneletLength()`to get a length of target lanelet, and `API::getMapPoseFromRelativePose()` to transform relative pose to map pose. | `traffic_simulator` | [#1145](https://github.com/tier4/scenario_simulator_v2/pull/1145) | [hakuturu583](https://github.com/hakuturu583) | +| Start supporting ego entity in C++ scenario | Enable spawn ego entity in C++ scenarios in cpp_mock_scenarios package. | `cpp_mock_scenarios` | [#1145](https://github.com/tier4/scenario_simulator_v2/pull/1145) | [hakuturu583](https://github.com/hakuturu583) | +| Visualize context in rviz display | Enable visualize `openscenario_interpreter_msgs/msg/Context` messages in rviz display | `openscenario_visualization` | [#1033](https://github.com/tier4/scenario_simulator_v2/pull/1033) | [kyoichi-sugahara](https://github.com/kyoichi-sugahara) | + +Bug Fixes:bug: + +| Feature | Brief summary | Category | Pull request | Contributor | +| ---------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------- | ----------------------------------------------------------------- | ----------------------------------------------- | +| Deleted entity management | Fix crash caused by `DeleteEntityAction` by handling deleted entity explicitly. | `traffic_simulator` | [#1096](https://github.com/tier4/scenario_simulator_v2/pull/1096) | [f0reachARR](https://github.com/f0reachARR) | +| OpenSCENARIO `RelativeDistanceCondition` | Restore the behavior of the existing distance measurement mode of RelativeDistanceCondition that was changed in #1095. | `openscenario_interpreter` | [#1121](https://github.com/tier4/scenario_simulator_v2/pull/1121) | [yamacir-kit](https://github.com/yamacir-kit) | +| Suppression of duplicate node names | Fix an issue where nodes with the same name were being created. (especially with temporary nodes for `UserDefinedAction.CustomCommandAction`) | `scenario_test_runner`, `openscenario_interpreter` | [#1126](https://github.com/tier4/scenario_simulator_v2/pull/1126) | [lchojnack](https://github.com/lchojnack) | +| Fix bugs in geometry library. | Fix some problems about geometry calculation. It includes breaking changes. Please also read [pull request](https://github.com/tier4/scenario_simulator_v2/pull/1139) | `geometry` | [#1126](https://github.com/tier4/scenario_simulator_v2/pull/1139) | [TauTheLepton](https://github.com/TauTheLepton) | + +Minor Tweaks :oncoming_police_car: + +| Feature | Brief summary | Category | Pull request | Contributor | +| -------------------------------------- | ----------------------------------------------------------------------------------------------------------- | ------------------- | ----------------------------------------------------------------- | ------------------------------------------------------------- | +| Additional V2I traffic light publisher | Add V2I traffic light publisher to cover the default value of AWF Autoware's external traffic lights topic. | `traffic_simulator` | [#1149](https://github.com/tier4/scenario_simulator_v2/pull/1149) | [piotr-zyskowski-rai](https://github.com/piotr-zyskowski-rai) | ## Version 0.8.0 diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 348f2d61e48..1157fd965c9 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -19,24 +19,27 @@ ### Publishers -| topic | type | note | -|---------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | -| `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | -| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | | -| `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | | -| `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/vehicle/status/control_mode` | [`autoware_auto_vehicle_msgs/msg/ControlModeReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl) | | -| `/vehicle/status/gear_status` | [`autoware_auto_vehicle_msgs/msg/GearReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearReport.idl) | | -| `/vehicle/status/steering_status` | [`autoware_auto_vehicle_msgs/msg/SteeringReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/SteeringReport.idl) | | -| `/vehicle/status/turn_indicators_status` | [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl) | | -| `/vehicle/status/velocity_status` | [`autoware_auto_vehicle_msgs/msg/VelocityReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/VelocityReport.idl) | | -| `/perception/object_recognition/detection/objects` | [`autoware_auto_perception_msgs/msg/DetectedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/DetectedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/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) | -| `/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) | | +| topic | type | note | +|------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `/clock` | [`rcl_interfaces/msg/Clock`](https://github.com/ros2/rcl_interfaces/blob/master/rosgraph_msgs/msg/Clock.msg) | | | | +| `/initialpose` | [`geometry_msgs/msg/PoseWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | | +| `/localization/kinematic_state` | [`nav_msgs/msg/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) | | +| `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | | +| `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | +| `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | +| `/vehicle/status/control_mode` | [`autoware_auto_vehicle_msgs/msg/ControlModeReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl) | | +| `/vehicle/status/gear_status` | [`autoware_auto_vehicle_msgs/msg/GearReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearReport.idl) | | +| `/vehicle/status/steering_status` | [`autoware_auto_vehicle_msgs/msg/SteeringReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/SteeringReport.idl) | | +| `/vehicle/status/turn_indicators_status` | [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl) | | +| `/vehicle/status/velocity_status` | [`autoware_auto_vehicle_msgs/msg/VelocityReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/VelocityReport.idl) | | +| `/perception/object_recognition/detection/objects` | [`autoware_auto_perception_msgs/msg/DetectedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/DetectedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/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) | +| `/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) | | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | [//]: # (| /rosout | rcl_interfaces/msg/Log | | |) [//]: # (| /tf | tf2_msgs/msg/TFMessage | | |) diff --git a/docs/developer_guide/OpenSCENARIOSupport.md b/docs/developer_guide/OpenSCENARIOSupport.md index 768e1ff5bd0..e71a23da9d5 100644 --- a/docs/developer_guide/OpenSCENARIOSupport.md +++ b/docs/developer_guide/OpenSCENARIOSupport.md @@ -1,6 +1,6 @@ # OpenSCENARIO Support -The ROS 2 package `openscenario_interpreter` provides scenario-based simulation on [ASAM OpenSCENARIO 1.0](https://www.asam.net/standards/detail/openscenario/). +The ROS 2 package `openscenario_interpreter` provides scenario-based simulation on [ASAM OpenSCENARIO 1.2](https://www.asam.net/standards/detail/openscenario-xml/). This section describes the differences between our OpenSCENARIO Interpreter and the OpenSCENARIO standard set by ASAM, and the OpenSCENARIO implementation by other companies and organizations. If you want to know about OpenSCENARIO, refer to the link below. diff --git a/docs/image/condition_group.png b/docs/image/condition_group.png new file mode 100644 index 00000000000..3d3cc5d2b1b Binary files /dev/null and b/docs/image/condition_group.png differ diff --git a/docs/user_guide/QuickStart.md b/docs/user_guide/QuickStart.md index 423abf34632..1203f12df49 100644 --- a/docs/user_guide/QuickStart.md +++ b/docs/user_guide/QuickStart.md @@ -80,7 +80,7 @@ This document contains step-by-step instruction on how to build and run [AWF Aut vehicle_model:=sample_vehicle ``` - To modify parameters of random testing, please check documentation of [random_test_runner](random_test_runner/README.md) + To modify parameters of random testing, please check documentation of [random_test_runner](random_test_runner/Usage.md) #### cpp scenario demo This demo does not use Autoware. the ego vehicle is interpreted as an NPC. diff --git a/docs/user_guide/VisualizingInternalInformation.md b/docs/user_guide/VisualizingInternalInformation.md new file mode 100644 index 00000000000..dffc38e84d9 --- /dev/null +++ b/docs/user_guide/VisualizingInternalInformation.md @@ -0,0 +1,56 @@ +# **Visualizing the Internal Information of the Simulator** + +This document provides instructions for users on how to visualize the internal information of `scenario_simulator_v2` using rviz. It also covers features that can be beneficial for analyzing this information. Please note, **this document is based on information up to version v0.7.0**. + +## Table of Contents + +- [**Visualizing the Internal Information of the Simulator**](#visualizing-the-internal-information-of-the-simulator) + - [Table of Contents](#table-of-contents) + - [**Visualizing Entity Information**](#visualizing-entity-information) + - [**Visualizing the Status of ConditionGroups**](#visualizing-the-status-of-conditiongroups) + +## **Visualizing Entity Information** + +WIP + +## **Visualizing the Status of ConditionGroups** + +`ConditionGroup`s defined in the scenario file can be checked on rviz, a visualization tool. + +A `ConditionGroup` acts as a trigger for events described in the scenario. A `ConditionGroup` is deemed true when all its sub-Conditions are fulfilled. Furthermore, event triggers can have multiple `ConditionGroup`s. If the evaluation of any one `ConditionGroup` becomes true, the trigger is activated. + +One usage example is to display `ConditionGroup`s at the moment the scenario fails. This allows you to analyze the cause of the failure. + +The display structure is as follows, and allows multiple Conditions to be included in one `ConditionGroup`: + +```markdown +Condition Group Name: xxx +Current Evaluation: xxx +Current Value: true/false +Type: ReachPositionCondition, CollisionCondition, etc. +Current Evaluation: +Current Value: +Type: +︙ +Condition Group Name: xxx +Current Evaluation: xxx +Current Value: true/false +Type: ReachPositionCondition, CollisionCondition, etc. +Current Evaluation: +Current Value: +Type: +︙ +``` + +Each condition is described using three elements: + +- **Current Evaluation**: Current achievement status +- **Current Value**: The fulfillment status of the condition expressed in true/false +- **Type**: Types of conditions, such as `ReachPositionCondition`, `CollisionCondition`, etc. + +To visualize the achievement status, check the `ConditionGroup` checkbox as demonstrated in the image below. +![condition group visualization](../image/condition_group.png "condition group visualization")") + +If an `Event name` is defined in the `ConditionGroup`, that name will be reflected. If left blank, names like `ConditionGroup1`, `ConditionGroup2`, `ConditionGroup3`, etc. are automatically assigned. However, this information alone doesn't clarify if each `ConditionGroup` is linked to success or failure conditions. Therefore, when visualizing with rviz and you want this information to be apparent, we recommend explicitly defining the `Event name` as `SuccessCondition` or similar when creating the scenario file. + +--- diff --git a/docs/user_guide/random_test_runner/Design.md b/docs/user_guide/random_test_runner/Design.md new file mode 100644 index 00000000000..b0738aa8e18 --- /dev/null +++ b/docs/user_guide/random_test_runner/Design.md @@ -0,0 +1,21 @@ +# Design + +This section describes the details of `RandomTestRunner` implementation and behavior. + +![Block diagram](img/block-diagram.jpg) + +Before the simulation starts, `RandomTestRunner` creates multiple `TestExecutors` for every test it needs to perform. In order to create `TestExecutor` runner uses `TestRandomizer::generate` method which returns `TestDescription`. The description includes information about ego start and goal position as well as the information about npcs. This generated test description is passed to the `TestExecutor`. The generation process is done separately for each test executor. + +In order to generate the test parameters `TestRandomizer` uses multiple `Randomizers`. It also uses `hdmap_utils::HdMapUtils` to operate on lanelets information. + +After the generation `RandomTestRunner` keeps the vector of test executors and inside `RandomTestRunner::update` method it initialize and update the executors sequentially. + +During the test `TestExecutor` uses the object of `JunitXmlReporter` class to report the errors. More detailed information about the kinds of reported error can be found in [Usage](Usage.md#result-junit-file). + +`TestExecutor` uses `traffic_simulator::API` to update the state of the simulation and validate the metrics. Executor finishes the test if the goal is reached, an unexpected standstill is detected or the scheduled time for the test has expired. + +After all the tests are finished `RandomTestRunner` calls `JUnitXMLReporter::write()` and information reported during the test is saved to the `report.junit.xml` file inside configured directory. The directory can be specified as `RandomTestRunner` ROS 2 parameter. To see the detailed description of the result file please see [Results](Usage.md#result-junit-file). + +To better visualize how this module works please find the sequence diagram below: + +![Sequence diagram](img/sequence-diagram.jpg) diff --git a/docs/user_guide/random_test_runner/QuickStart.md b/docs/user_guide/random_test_runner/QuickStart.md new file mode 100644 index 00000000000..d048b20f9d6 --- /dev/null +++ b/docs/user_guide/random_test_runner/QuickStart.md @@ -0,0 +1,110 @@ +# Random test runner + +Random test runner allows running randomly generated scenarios to test Autoware autonomy implementation. For more information regarding Random test runner features and limitations please see [Usage](Usage.md#features). + +## How to build + +1. Clone the Autoware Core/Universe repository: + ```bash + git clone git@github.com:autowarefoundation/autoware.git + ``` +2. Navigate to the source directory: + ```bash + cd autoware + mkdir src + ``` +3. Import Autoware and Simulator dependencies: + ```bash + vcs import src < autoware.repos + vcs import src < simulator.repos + ``` +4. Install dependencies for Autoware Core/Universe + ```bash + ./setup-dev-env.sh + ``` + +5. Install dependent ROS packages. + ```bash + source /opt/ros/humble/setup.bash + rosdep install -iry --from-paths src --rosdistro $ROS_DISTRO + ``` +6. Build the workspace. + ```bash + colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release + ``` + +## How to run + +First complete build described in [How to build](#how-to-build) section. + + +Being in the main project directory run: + +```shell +source install/setup.bash +ros2 launch random_test_runner random_test.launch.py +``` + +Which will run random tests with default parameters. Similarly to the image below you should see several npcs spawned in random locations around the ego vehicle, which will move on random path following the goal. + +![Random test runner launched](img/random-test-runner-launched.png) + +Detailed description of the possible parameters can be found under [Parameters](Usage.md#launch-arguments). + +## + +After test is completed see `/tmp` directory. Among others, there will be two files: +1. `result.junit.xml` - test result file with information about encountered errors. +2. `result.yaml` - yaml file that can be used to replay tests. + +For the more specified information about output files please see [Results](Usage.md#results). + +## + +It might happen that the random test runner will behave unexpectedly and the test will not launch correctly. For further details regarding known issues please see [Troubleshooting](Usage.md#troubleshooting). + +## How to replay + +Prerequisites: +1. Build as instructed in [How to build](#how-to-build) +2. Acquire `result.yaml` file: + 1. Either by running test as stated in [How to run](#how-to-run) part of instruction. + 2. Receiving it from someone who already ran it. +3. Place `result.yaml` in `` IMPORTANT: Do not change filename. +4. Execute: + +```shell +ros2 launch random_test_runner random_test.launch.py input_dir:= +``` + +Random test runner will load `result.yaml` file and rerun test. + +## Running with unity + +TBD + +[//]: # (Instruction is based on `kashiwanoha_map` Unity project but can be applied to any other projects supporting [`ZeroMQ` interface](https://tier4.github.io/scenario_simulator_v2-docs/design/ZeroMQ/). ) + +[//]: # () +[//]: # (To run `random_test_runner` with Unity Kashiwanoha project: ) + +[//]: # (1. Clone and run [Kashiwanoha project](https://gitlab.com/robotec.ai/tieriv/kashiwanoha).) + +[//]: # (2. Make sure that package name in `map_name` parameter is `kashiwanoha_map`. For projects other than Kashiwanoha, make sure to change it to correct package name. ) + +[//]: # (3. Execute `random_test_runner` launch with `simulator_type` parameter:) + +[//]: # (```shell) + +[//]: # (ros2 launch random_test_runner random_test.launch.py simulator_type:="unity") + +[//]: # (```) + +[//]: # (| NOTE: Since currently unity integration does not support ego vehicle, `random_test_runner` does not spawn it. |) + +[//]: # (|----------------------------------------------------------------------------------------------------------------|) + +[//]: # () +[//]: # (| NOTE: Kashiwanoha project is only supported on ROS 2 `galactic` but simulation interfaces are distribution-independent and random tests can be executed safely on `foxy` |) + +[//]: # (|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------|) diff --git a/docs/user_guide/random_test_runner/README.md b/docs/user_guide/random_test_runner/README.md deleted file mode 100644 index f53adfdef73..00000000000 --- a/docs/user_guide/random_test_runner/README.md +++ /dev/null @@ -1,228 +0,0 @@ -# Random test runner - -Random test runner allows running randomly generated scenarios to test autoware autonomy implementation. - -[//]: # (TODO : remove .Auto) -| NOTE: Currently only `Autoware.Auto` (`awf/auto`) is supported. | -|-----------------------------------------------------------------| - -## How to build - -[//]: # (TODO : remove .Auto) -### Autoware.Auto - -It is assumed that you are running Autoware.Auto in ADE environment with default paths. See [Autoware.Auto documentation](https://autowarefoundation.gitlab.io/autoware.auto/AutowareAuto/installation-ade.html) for instructions how to correctly set up Autoware.Auto and ADE. - -Inside Autoware.Auto directory run and enter ade: -```shell -ade --rc .aderc-amd64-foxy-lgsvl start --update --enter -``` - -enter Autoware.Auto directory, install required dependencies and build - -```shell -cd -vcs import < autoware.auto.foxy.repos -./tools/simulation/get_scenario_simulator_v2.sh -colcon build --packages-up-to random_test_runner scenario_simulator_launch kashiwanoha_map --cmake-args -DCMAKE_BUILD_TYPE=Release -``` - -## How to run - -First complete build described in [How to build](#how-to-build) section for autoware architecture you are using. - -Then, in first terminal run: - -```shell -source install/setup.bash -rviz2 -d $(ros2 pkg prefix random_test_runner)/share/random_test_runner/rviz/random_test.rviz -``` - -In second: - -```shell -source install/setup.bash -ros2 launch random_test_runner random_test.launch.py -``` - -Which will run random tests with default parameters. You should see several npcs spawned in random locations around the ego vehicle, which will move on random path. - -After test is completed see `/tmp` directory. Among others, there will be two files: -1. `result.junit.xml` - test result file with information about encountered errors -2. `result.yaml` - yaml file that can be used to replay tests - -Exemplary content of those files can be seen below: - -`result.junit.xml`: -```xml - - - - - - - - - - - - - - - - - - - - -``` - -`result.yaml`: -```yaml -random_test: - name: video_test - map_name: kashiwanoha_map - ego_goal_s: 5.000000000000000000 - ego_goal_lanelet_id: -1 - ego_goal_partial_randomization: false - ego_goal_partial_randomization_distance: 30 - npc_count: 10 - npc_min_speed: 0.500000000000000000 - npc_max_speed: 3.000000000000000000 - npc_min_spawn_distance_from_ego: 10.000000000000000000 - npc_max_spawn_distance_from_ego: 50.000000000000000000 - test_cases: - - seed: 1281242544 - - seed: 3644198185 - - seed: 2673087374 - - seed: 1312244741 - - seed: 2518911638 -``` - -## How to replay - -Prerequisites: -1. Build as instructed in [How to build](#how-to-build) -2. Acquire `result.yaml` file: - 1. Either by running test as stated in [How to run](#how-to-run) part of instruction - 2. Receiving it from someone who already ran it -3. Place `result.yaml` in `` IMPORTANT: Do not change filename -4. Execute: - -```shell -ros2 launch random_test_runner random_test.launch.py input_dir:= -``` - -Random test runner will load `result.yaml` file and rerun test. - -## Running with unity - -TBD - -[//]: # (Instruction is based on `kashiwanoha_map` Unity project but can be applied to any other projects supporting [`ZeroMQ` interface](https://tier4.github.io/scenario_simulator_v2-docs/design/ZeroMQ/). ) - -[//]: # () -[//]: # (To run `random_test_runner` with Unity Kashiwanoha project: ) - -[//]: # (1. Clone and run [Kashiwanoha project](https://gitlab.com/robotec.ai/tieriv/kashiwanoha).) - -[//]: # (2. Make sure that package name in `map_name` parameter is `kashiwanoha_map`. For projects other than Kashiwanoha, make sure to change it to correct package name. ) - -[//]: # (3. Execute `random_test_runner` launch with `simulator_type` parameter:) - -[//]: # (```shell) - -[//]: # (ros2 launch random_test_runner random_test.launch.py simulator_type:="unity") - -[//]: # (```) - -[//]: # (| NOTE: Since currently unity integration does not support ego vehicle, `random_test_runner` does not spawn it. |) - -[//]: # (|----------------------------------------------------------------------------------------------------------------|) - -[//]: # () -[//]: # (| NOTE: Kashiwanoha project is only supported on ROS 2 `galactic` but simulation interfaces are distribution-independent and random tests can be executed safely on `foxy` |) - -[//]: # (|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------|) - -## Launch arguments - -This section describes arguments accepted by launch file. Please note that those arguments should not be included in parameters file specified in `test_parameters_filename`. - -Apart from that, any `random_test_runner` node parameters can be supplied as a launch arguments. -Please refer to [Node parameters](#node-parameters) chapter for more details on parameter source priorities. - -### General arguments - -| Parameter name | Default value | Description | -|------------------------------|-------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| `test_parameters_filename` | `""` | Yaml filename within `random_test_runner/param directory` containing test parameters. With exception from [Launch arguments](#launch-arguments) chapter, parameters specified here will override values passed as arguments | -| `simulator_type` | `"simple_sensor_simulator"` | Backend simulator. Supported values are `unity` and `simple_sensor_simulator`. It's also accepted by the node but should be supplied as direct launch argument | -|`simulator_host` | `"localhost"` | Simulation host. It can be either IP address or the host name that is resolvable in the environment (you can add a host by appending `" "` line to the `/etc/hosts` file) | - -### Autoware related arguments - -Launch also accepts autoware parameters that control autoware related behavior. It can set which autoware architecture is in use, which vehicle -and sensor model is used in the simulation - -| Parameter name | Default value | Description | -|---------------------|-------------------------------|---------------------------------------------------------------------------| -| `architecture_type` | `"awf/auto"` | Autoware architecture type. Possible values: `awf/auto`, `tier4/proposal` | -| `sensor_model` | `"aip_xx1"` | Ego sensor model | -| `vehicle_model` | `"lexus"` | Ego vehicle model | - - -## Node parameters - -Random testing supports several parameters to control test execution. They can be supplied either directly from command -line or via yaml file inside `/param/` named `test_parameters_filename`. - -Parameters have the following source precedence priorities: -1. `*.yaml` file -2. Command line -3. Default values - -### Parameters reference - -Random test runner parameters are split into three categories: - -1. [Test control parameters](#test-control-parameters) -2. [Test suite parameters](#test-suite-parameters) -3. [Test case parameters](#test-case-parameters) - -#### Test control parameters - -High level parameters not directly related to the test itself - -| Parameter name | Default value | Description | -|-------------------|-------------------------------|---------------------------------------------------------------------------------------------------------------------------| -| `input_dir` | `""` | Directory containing the result.yaml file to be replayed. If not empty, tests will be replayed from result.yaml | -| `output_dir` | `"/tmp"` | Directory to which result.yaml and result.junit.xml files will be placed | -| `test_count` | `5` | Number of test cases to be performed in the test suite | -| `simulator_type` | `"simple_sensor_simulator"` | Backend simulator. Supported values are `unity` and `simple_sensor_simulator`. It should be set only via launch argument | - -#### Test suite parameters - -Core test parameters. It sets map name, ego goal information and npc spawning parameters. - -| Parameter name | Default value | Description | -|-------------------------------------------|---------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| `test_name` | `"random_test"` | Test name. Used for descriptive purposes only | -| `map_name` | `"kashiwanoha_map"` | Package name containing map information (lanelet, point cloud, etc) | -| `ego_goal_lanelet_id` | `-1` | Goal lanelet's id. If `-1`, goal will be chosen randomly | -| `ego_goal_s` | `0.0` | Goal lanelet's s (translation along the lanelet in meters). If `ego_goal_lanelet_id` equals `-1`, s will be chosen randomly | -| `ego_goal_partial_randomization` | `False` | If `true`, goal will be randomized within distance set in `ego_goal_partial_randomization_distance` value. If `ego_goal_lanelet_id` is set to `-1`, this value is ignored | -| `ego_goal_partial_randomization_distance` | `30.0` | Distance in meters from goal set by `ego_goal_lanelet_id` and `ego_goal_s`, within which goal pose will be randomized if `ego_goal_partial_randomization` is set to true | -| `npc_count` | `10` | Generated npc count | -| `npc_min_speed` | `0.5` | Minimum speed of generated npcs | -| `npc_max_speed` | `3.0` | Maximum speed of generated npcs | -| `npc_min_spawn_distance_from_ego` | `10.0` | Minimum distance of generated npcs from ego | -| `npc_max_spawn_distance_from_ego` | `100.0` | Maximum distance of generated npcs from ego | - -#### Test case parameters - -Test case parameters. Currently, only randomization seed. - -| Parameter name | Default value | Description | -|-----------------|---------------|-------------------------------------------------------------------| -| `seed` | `-1` | Randomization seed. If `-1`, seed will be generated for each test | diff --git a/docs/user_guide/random_test_runner/Usage.md b/docs/user_guide/random_test_runner/Usage.md new file mode 100644 index 00000000000..aaf527cc496 --- /dev/null +++ b/docs/user_guide/random_test_runner/Usage.md @@ -0,0 +1,239 @@ +# Features + +- Full and partial randomization of the Ego goal position for multiple test cases. + +- NPCs customization: + - number of NPCs + - distance from the Ego + - velocities of NPC vehicles in the simulation. + +- Simulation related errors reporting: + - Ego being found stuck in one place for too long. + - Ego failing to reach the goal in a specified time. + - Collision between Ego and NPC. + +- Ego vehicle capabilities: + - Goal following + - Lane changing + - Stopping precisely on a designated line + - Yield at intersections + +- NPCs possible actions: + - Following the line. + - Following another entity. + +# Limitations + +- Random test runner does not support traffic lights signalization. + +- NPCs actions: + - NPCs are not stopping to avoid collision when they are not following the colliding entity (eg. on the intersections). + - NPCs are not able to yield at the intersection. + + +# Troubleshooting + +There are two known issues which block random test runner from initializing the test if either initial position or goal pose exceeds the drivable area. + +- If the initial position exceeds the drivable are: + +Autoware will be launched, but the ego vehicle will not move, waiting for Autoware to change its state to WaitingForEngage. After a timeout the test will terminate and the runner will continue with the next test. + +After the test suite is completed, the `AutowareError` connected with the failed test will be logged to the `result.junit.xml` with the message: `Simulator waited for the Autoware state to transition to WaitingForEngage, but time is up. The current Autoware state is EMERGENCY.` + +- If the generated goal pose exceeds the drivable ares: + +The test will be immediately stopped after launching the Autoware and similarly as in the first case the error will be logged and the runner will continue with the next test. + +After the test suite is completed, the `scenario_simulator_error` connected with the failed test will be logged to the `result.junit.xml` with the message: `Requested the service "/api/routing/set_route_points" 1 times, but was not successful.` + +# Launch arguments + +This section describes arguments of the random test runner. All of them can be specified via command line, otherwise default value specified in the launch file is used. + +Parameters listed in [Node parameters](#node-parameters) section can be also specified in a yaml file which should be located in `/param/`. + +Parameters have the following source precedence priorities: +1. `*.yaml` file (applicable only for [Node parameters](#node-parameters)) +2. Command line +3. Default values + +## General arguments + +| Parameter name | Default value | Description | +|------------------------------|-------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `test_parameters_filename` | `""` | Yaml filename within `random_test_runner/param` directory containing test parameters. Only [Node parameters](#node-parameters) can be specified in this file. | +| `simulator_type` | `"simple_sensor_simulator"` | Backend simulator. Currently supported value is `simple_sensor_simulator`. It's also accepted by the node but should be supplied as direct launch argument. | +| `simulator_host` | `"localhost"` | Simulation host. It can be either IP address or the host name that is resolvable in the environment (you can add a host by appending `" "` line to the `/etc/hosts` file). | + +## Autoware related arguments + +Launch also accepts Autoware parameters that control Autoware related behavior. It can set which Autoware architecture is in use, which vehicle +and sensor model is used in the simulation + +| Parameter name | Default value | Description | +|---------------------|-------------------------------|---------------------------------------------------------------------------| +| `architecture_type` | `"awf/universe"` | Autoware architecture type. Currently the only supported value is: `awf/universe`. | +| `sensor_model` | `"sample_sensor_kit"` | Ego sensor model. | +| `vehicle_model` | `"sample_vehicle"` | Ego vehicle model. | + + +## Node parameters + +Random testing supports several parameters to control test execution. They can be supplied either directly from command +line or via `*.yaml` file inside `/param/`. Example of the yaml file specifying parameters can be found [here](#example-parameters-yaml-file). + +### Parameters reference + +Random test runner parameters are split into three categories: + +1. [Test control parameters](#test-control-parameters) +2. [Test suite parameters](#test-suite-parameters) +3. [Test case parameters](#test-case-parameters) + +### Test control parameters + +High level parameters not directly related to the test itself + +| Parameter name | Default value | Description | +|-------------------|-------------------------------|---------------------------------------------------------------------------------------------------------------------------| +| `input_dir` | `""` | Directory containing the result.yaml file to be replayed. If not empty, tests will be replayed from result.yaml. | +| `output_dir` | `"/tmp"` | Directory in which result.yaml and result.junit.xml files will be placed after the test suite is executed. | +| `test_count` | `5` | Number of test cases to be performed in the test suite. | +| `simulator_type` | `"simple_sensor_simulator"` | Backend simulator. Currently supported value is `simple_sensor_simulator`. It should be set only via launch argument. | +| `initialize_duration` | `35` | How long test runner will wait for Autoware to initialize. | + +### Test suite parameters + +Core test parameters. It sets map name, ego goal information and npc spawning parameters. + +| Parameter name | Default value | Description | +|-------------------------------------------|---------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| `test_name` | `"random_test"` | Test name. Used for descriptive purposes only. | +| `map_name` | `"kashiwanoha_map"` | Name of the package containing map information. For more information please see [map package](Usage.md#map-package). | +| `ego_goal_lanelet_id` | `-1` | Goal lanelet's id. If `-1`, goal will be chosen randomly. | +| `ego_goal_s` | `0.0` | Goal lanelet's translation along the lanelet in meters. For more detailed description please see [lanelets positioning](Usage.md#lanelets-positioning). If `ego_goal_lanelet_id` equals `-1`, s will be chosen randomly. | +| `ego_goal_partial_randomization` | `False` | If `true`, goal will be randomized within distance set in `ego_goal_partial_randomization_distance` value. For more detailed description please see [lanelets positioning](Usage.md#lanelets-positioning). If `ego_goal_lanelet_id` is set to `-1`, this value is ignored. | +| `ego_goal_partial_randomization_distance` | `25.0` | Distance in meters from goal set by `ego_goal_lanelet_id` and `ego_goal_s`, within which goal pose will be randomized if `ego_goal_partial_randomization` is set to true. | +| `npc_count` | `10` | Number of npcs which will be generated. | +| `npc_min_speed` | `0.5` | Minimum speed of generated npcs | +| `npc_max_speed` | `3.0` | Maximum speed of generated npcs | +| `npc_min_spawn_distance_from_ego` | `10.0` | Minimum distance of generated npcs from ego | +| `npc_max_spawn_distance_from_ego` | `100.0` | Maximum distance of generated npcs from ego | +#### Map package + +Package containing map information. This package needs to be built in the Autoware workspace. Package example can be found [here](https://github.com/tier4/AWSIM/releases/download/v1.2.0/shinjuku_map.zip). + +The structure of the package needs to be as follows: + +- The root package directory, which contains: + - `package.xml` file + - `CMakeList.txt` file + - map directory, which contains: + - `.osm` file + - `.pcd` file + +#### Lanelets positioning + +Lanelet position used to represent the ego goal includes two coordinate: + - the ID of the lanelet on which the target is located, + - the translation along the lanelet. + +On the example below two connected lanelets with IDs 101 and 100 are presented. The translation s is the distance between lanelet's beginning and the vehicle's rear axle position. + +![Lanelets positioning](img/lanelet.jpg) + +### Test case parameters + +Test case parameters. Currently, only randomization seed. + +| Parameter name | Default value | Description | +|-----------------|---------------|-------------------------------------------------------------------| +| `seed` | `-1` | Randomization seed. If `-1`, seed will be generated for each test | + +### Example parameters yaml file +``` +/**: + ros__parameters: + # control parameters + "output_dir": "/tmp" + "input_dir": "" + + # test suite parameters + "test_name": "random_test" + "map_name": "kashiwanoha_map" + "ego_goal_lanelet_id": 34600 # -1 means full goal randomization + "ego_goal_s": 5.0 +``` + +# Results + +After the test suite execution two files can be found in the specified output folder. + +## Result yaml file + +Stores parameters used to generate the test suite. This file might be used to rerun the tests as described in [Replay](QuickStart.md#how-to-replay). + +### Example `result.yaml`: +```yaml +random_test: + name: video_test + map_name: kashiwanoha_map + ego_goal_s: 5.000000000000000000 + ego_goal_lanelet_id: -1 + ego_goal_partial_randomization: false + ego_goal_partial_randomization_distance: 30 + npc_count: 10 + npc_min_speed: 0.500000000000000000 + npc_max_speed: 3.000000000000000000 + npc_min_spawn_distance_from_ego: 10.000000000000000000 + npc_max_spawn_distance_from_ego: 50.000000000000000000 + test_cases: + - seed: 1281242544 + - seed: 3644198185 + - seed: 2673087374 + - seed: 1312244741 + - seed: 2518911638 +``` + +## Result JUnit file + +The file contains information about errors which occurred during test cases which were executed by the random test runner. + +If the execution of the test finished with an error the stored information contains type of the error and the message describing the error. + +There are 3 types of error reported, related directly to the simulation, which are: +1. `Stand still error` - reported when ego is found stuck in one place for too long. +2. `Timeout error` - reported when ego fails to reach the goal in a specified time. +3. `Collision error` - reported when collision between ego and npc appears, +4. Exceptions caught during the runtime which includes: + - `AutowareError`, + - `scenario_simulator_exception`, + - `std::runtime_error`. + + +If any other error occurs during the random test runner execution, it will be stored along with the information that the `Unknown Error` has occurred. + +### Example `result.junit.xml`: +```xml + + + + + + + + + + + + + + + + + + + + +``` diff --git a/docs/user_guide/random_test_runner/img/block-diagram.jpg b/docs/user_guide/random_test_runner/img/block-diagram.jpg new file mode 100644 index 00000000000..66766b59747 Binary files /dev/null and b/docs/user_guide/random_test_runner/img/block-diagram.jpg differ diff --git a/docs/user_guide/random_test_runner/img/lanelet.jpg b/docs/user_guide/random_test_runner/img/lanelet.jpg new file mode 100644 index 00000000000..96ce3dbaabc Binary files /dev/null and b/docs/user_guide/random_test_runner/img/lanelet.jpg differ diff --git a/docs/user_guide/random_test_runner/img/random-test-runner-launched.png b/docs/user_guide/random_test_runner/img/random-test-runner-launched.png new file mode 100644 index 00000000000..36883345674 Binary files /dev/null and b/docs/user_guide/random_test_runner/img/random-test-runner-launched.png differ diff --git a/docs/user_guide/random_test_runner/img/sequence-diagram.jpg b/docs/user_guide/random_test_runner/img/sequence-diagram.jpg new file mode 100644 index 00000000000..6915a1da966 Binary files /dev/null and b/docs/user_guide/random_test_runner/img/sequence-diagram.jpg differ diff --git a/docs/user_guide/scenario_test_runner/ScenarioTestRunner.md b/docs/user_guide/scenario_test_runner/ScenarioTestRunner.md index fd5ba59a8e3..24a197ce734 100644 --- a/docs/user_guide/scenario_test_runner/ScenarioTestRunner.md +++ b/docs/user_guide/scenario_test_runner/ScenarioTestRunner.md @@ -3,7 +3,7 @@ You can test run (preview) the scenarios you created using the GUI scenario editor before running it with Autoware. The file you exported from the GUI scenario editor is in YAML based format called "[TIER IV Scenario Format Version 2.0](../../developer_guide/TIERIVScenarioFormatVersion2.md)". Then it is converted into XML based OpenSCENARIO format. -The specification of this OpenSCENARIO format is found at [OpenSCENARIO](http://www.openscenario.org/) site. +The specification of this OpenSCENARIO format is found at [OpenSCENARIO](https://www.asam.net/standards/detail/openscenario-xml/) site. ## Before Testing Scenarios diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 25b75878e26..289fb3004a4 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -2,6 +2,154 @@ Changelog for package concealer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge pull request `#1077 `_ from tier4/fix/autoware-shutdown + Fix/autoware shutdown +* Add new member function `StatusMonitor::overrideThreshold` +* Lipsticks +* Update to kill process group if Autoware launch process is unresponsive +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Setting concealer use_sim_time manually instead of using global arguments. +* Global real time factor set with launch argument fix +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Cleanup +* Cleanup error messages +* Update `shutdownAutoware` to respect the parameter `sigterm_timeout` +* Add some comments +* Contributors: Masaya Kataoka, Paweł Lech, Tatsuya Yamasaki, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#1113 `_ from tier4/feature/doxygen +* update Doxyfile +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#1106 `_ from tier4/fix/rtc_command_action/continuous_execution +* Merge pull request `#997 `_ from tier4/feature/allow-goal-modification +* doc(concealer): add notes for RTC used status filter +* refactor: service_with_validation.hpp +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into fix/rtc_command_action/continuous_execution +* Merge pull request `#1092 `_ from tier4/feature/control_rtc_auto_mode +* refactor(concealer): delete debug print +* chore(concealer): delete unnecessary line break +* chore(concealer): delete unnecessary line break +* Merge pull request `#1099 `_ from tier4/pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* unnecesary check removed +* fix(concealer): fix capture +* refactor(concealer): add const to requestAutoModeForCooperation +* refactor(concealer): mark isPackageExists as noexcept +* chore(concealer): roll back the number of retries for setVelocityLimit +* chore: update comments +* Move some metafunctions into new header files +* Update `plan` to check that `SetRoutePoints::Requst` has the data member `option` +* removed leftover include +* Cleanup +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* fix(concealer): ignore timeout with engage service +* fix(concealer): set long validation interval for /api/routing/set_route_points +* feat(concealer): support modifying validation interval of ServiceWithValidation +* fix(concealer): allow timeout for requestInitialPose +* feat(concealer): add timeout exception class for ServiceWithValidation +* feat(concealer): enable timeout validation switching for ServiceWithValidation +* feat(concealer): add support for autoware_adapi_v1_msgs::msg::ResponseStatus for service response validation in ServiceWithValidation +* fix(concealer): revive ServiceWithValidation::validateAvailability and fix logic +* added has_include condition for localization state related code +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* chore(concealer): modify maximum number of retry for set velocity limit service +* refactor: service_with_validation.hpp +* refactor(concealer): organize order of class member variable +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* refactor(concealer): organize order of class member variable +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* chore: apply linter +* refactor(concealer): add is_package_exists.hpp +* refactor(concealer): stop using utility directory +* fix(concealer): fix the history filter of cooperate statuses +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* fix(concealer): fix build errors +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into fix/port_document +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1060 `_ from tier4/AJD-805/baseline_update_rebased +* fix(concealer): fix failure handling for service response +* clang format +* sending initial pose only when localizaiton is uninitialized +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* fix(concealer): add history filter for rtc command +* chore: apply linter +* feat(concealer): use task_queue to send RTC service request +* feat(concealer): import latest ServiceWithValidation class from RobotecAI's work +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Apply review changes +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* refactor: delete cooperator +* fix(concealer): fix compile errors +* feat(concealer): update interface for rtc auto mode +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* feat(concealer): add empty implementation of requestAutoModeForCooperation +* refactor(concealer): delete concealer::cooperate, approve +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* fix clang format error +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* remove workbound for galactic +* sending initial pose rate limited +* checkpoints reintroduced +* brought back velocity limit and cooperate commands +* commented clock and parts of concealer +* Run clang format +* Delete comments and new lines +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Regression: revert change operation mode to engage due to engage request after stopping +* Regression: fix autoware state naming +* Regression: stop overriding mrm state and behavior +* Update /autoware/state to /api/iv_msgs/autoware/state OLD AD API +* Implement /api/routing/set_route_points AD API +* Update getTrajectory topic type +* Revert "WIP: get and sets for vehicle status" +* WIP: get and sets for vehicle status +* Implement /api/external/get/emergency topic from tier4 external api +* Revert "WIP: add ADAPI support in service with validation" +* Implement /api/localization/initialize service for position initialization +* Implement /api/operation_mode/change_to_autonomous to engage +* WIP: add ADAPI support in service with validation +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Cleanup +* Update `concealer` to use service SetRoutePoints +* Add the property `allowGoalModification` to switch destination topics +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 83a033daefd..ef23df4b27d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 0.8.0 + 1.0.0 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 @@ -27,6 +27,7 @@ ament_index_cpp geometry_msgs + libboost-dev nav_msgs rclcpp scenario_simulator_exception @@ -34,7 +35,6 @@ tf2_geometry_msgs tf2_ros traffic_simulator_msgs - libboost-dev ament_cmake_clang_format ament_cmake_copyright diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index acccd31fee0..b1d658b9bab 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -16,6 +16,7 @@ #include #include #include +#include namespace concealer { @@ -70,71 +71,76 @@ auto FieldOperatorApplication::checkAutowareProcess() -> void auto FieldOperatorApplication::shutdownAutoware() -> void { - AUTOWARE_INFO_STREAM("Shutting down Autoware: (1/3) Stop publishing/subscribing."); - { - stopRequest(); - } - - if (process_id != 0 && not is_autoware_exited) { - is_autoware_exited = true; - - AUTOWARE_INFO_STREAM("Shutting down Autoware: (2/3) Send SIGINT to Autoware launch process."); - { - sendSIGINT(); - } - - AUTOWARE_INFO_STREAM("Shutting down Autoware: (2/3) Terminating Autoware."); - { - sigset_t mask{}; - { - sigset_t orig_mask{}; - - sigemptyset(&mask); - sigaddset(&mask, SIGCHLD); - - if (sigprocmask(SIG_BLOCK, &mask, &orig_mask) < 0) { - AUTOWARE_SYSTEM_ERROR("sigprocmask"); - std::exit(EXIT_FAILURE); - } - } - - timespec timeout{}; - { - timeout.tv_sec = 5; - timeout.tv_nsec = 0; + if (stopRequest(); process_id != 0 && not std::exchange(is_autoware_exited, true)) { + const auto sigset = [this]() { + if (auto signal_set = sigset_t(); + sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } else if (auto error = pthread_sigmask(SIG_BLOCK, &signal_set, nullptr)) { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(error, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } else { + return signal_set; } - - while (sigtimedwait(&mask, NULL, &timeout) < 0) { - switch (errno) { - case EINTR: // Interrupted by a signal other than SIGCHLD. - break; - - case EAGAIN: - AUTOWARE_ERROR_STREAM( - "Shutting down Autoware: (2/3) Autoware launch process does not respond. Kill it."); - kill(process_id, SIGKILL); - break; - - default: - AUTOWARE_SYSTEM_ERROR("sigtimedwait"); - std::exit(EXIT_FAILURE); - } + }(); + + const auto timeout = [this]() { + auto sigterm_timeout = [this](auto value) { + auto node = rclcpp::Node("get_parameter_sigterm_timeout", "simulation"); + node.declare_parameter("sigterm_timeout", value); + node.get_parameter("sigterm_timeout", value); + return value; + }; + auto timeout = timespec(); + timeout.tv_sec = sigterm_timeout(5); + timeout.tv_nsec = 0; + return timeout; + }(); + + if (sendSIGINT(); sigtimedwait(&sigset, nullptr, &timeout) < 0) { + switch (errno) { + case EINTR: + /* + The wait was interrupted by an unblocked, caught signal. It shall + be documented in system documentation whether this error causes + these functions to fail. + */ + RCLCPP_ERROR_STREAM( + get_logger(), + "The wait for Autoware launch process termination was interrupted by an unblocked, " + "caught signal."); + break; + + case EAGAIN: + /* + No signal specified by set was generated within the specified + timeout period. + */ + RCLCPP_ERROR_STREAM(get_logger(), "Autoware launch process does not respond. Kill it."); + killpg(process_id, SIGKILL); + break; + + default: + case EINVAL: + /* + The timeout argument specified a tv_nsec value less than zero or + greater than or equal to 1000 million. + */ + RCLCPP_ERROR_STREAM( + get_logger(), + "The parameter sigterm_timeout specified a value less than zero or greater than or " + "equal to 1000 million."); + break; } } - AUTOWARE_INFO_STREAM("Shutting down Autoware: (3/3) Waiting for Autoware to be exited."); - { - int status = 0; - - const int waitpid_options = 0; - - if (waitpid(process_id, &status, waitpid_options) < 0) { - if (errno == ECHILD) { - AUTOWARE_WARN_STREAM("Try to wait for the autoware process but it was already exited."); - } else { - AUTOWARE_SYSTEM_ERROR("waitpid"); - std::exit(EXIT_FAILURE); - } + if (int status = 0; waitpid(process_id, &status, 0) < 0) { + if (errno == ECHILD) { + RCLCPP_ERROR_STREAM( + get_logger(), "Try to wait for the autoware process but it was already exited."); + } else { + RCLCPP_ERROR_STREAM(get_logger(), std::system_error(errno, std::system_category()).what()); } } } diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index a619ea8ef60..41519fd31f1 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package kashiwanoha_map ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 76c26ee2cb6..e77e8896b99 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 0.8.0 + 1.0.0 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/mkdocs.yml b/mkdocs.yml index c1be9de4a50..19eebffda96 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -56,7 +56,9 @@ extra_javascript: nav: - Home: README.md - - Release Note: ReleaseNotes.md + - Release Note: + - Since version 1.0.0: https://github.com/tier4/scenario_simulator_v2/releases + - Prior to version 1.0.0: ReleaseNotes.md - User Guide: - Quick Start: user_guide/QuickStart.md - Build Instructions: user_guide/BuildInstructions.md @@ -64,12 +66,15 @@ nav: - Scenario Writing Tips: user_guide/ScenarioTips.md - Scenario Editor: - Overview: user_guide/scenario_editor/ScenarioEditorUserGuide.md - - Place an entity to relative position: user_guide/scenario_editor/RelativePosition.md - Scenario Test Runner: - Overview: user_guide/scenario_test_runner/ScenarioTestRunner.md - user_guide/scenario_test_runner/ScenarioFormatConversion.md - user_guide/scenario_test_runner/HowToWriteWorkflowFile.md - user_guide/scenario_test_runner/Tips.md + - Random Test Runner: + - Quick Start: user_guide/random_test_runner/QuickStart.md + - Usage: user_guide/random_test_runner/Usage.md + - Design: user_guide/random_test_runner/Design.md - Developer Guide: - About: developer_guide/About.md - developer_guide/AutowareAPI.md diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 909f149ab85..a8eb72b8448 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -2,6 +2,110 @@ Changelog for package cpp_mock_scenarios ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge pull request `#1139 `_ from tier4/fix/geometry-bug-fixes +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge remote-tracking branch 'origin/master' into feature/traffic-lights-awsim-support +* Merge pull request `#1145 `_ from tier4/feature/random_scenario +* modify default sensor model +* change default vehicle model +* remove unused lambda function +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* fix typo +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Remove unnecessary comments +* Fix lanechange time constraint scenarios +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* remove debug line +* remove function object +* add spawn_nearby_ego entity +* Merge pull request `#1113 `_ from tier4/feature/doxygen +* fix compile error +* add namespace +* update namespace +* add concealer +* add spawn outside vehicle +* overwrite label from parameter +* enable set label +* rename scenario class +* add namespace +* rename scenario classes +* add namespace +* add cpp_mock_scenarios +* remove debug lines +* Merge remote-tracking branch 'origin' into feature/RJD-96_detail_message_scenario_failure +* remove sending route function +* remove sending route +* enable run scenario +* update scenario +* Merge branch 'master' into AJD-805/baseline_update_rebased +* add comment +* uncomment targets +* fix scenario +* randomize speed +* despawn stopped pedestrian +* add offset variance parameter +* add s variance +* enable clean up entity +* set bounds +* enable update parameter +* add parameters +* modify condition +* update scenario +* update scenario +* update scenario +* add random scenario +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge pull request `#1080 `_ from tier4/doc/add_comment_for_pr_1074 +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* remove workbound for galactic +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* add comment about PR `#1074 `_ +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into ref/RJD-553_restore_repeated_update_entity_status diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 58e04a4b21a..bb16b4bf1e4 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 0.8.0 + 1.0.0 C++ mock scenarios masaya Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp index 053b5d0b841..a0581369b18 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp @@ -47,7 +47,9 @@ class LaneChangeLinearTimeScenario : public cpp_mock_scenarios::CppScenarioNode lanechange_frames++; } if (api_.getCurrentAction("ego") != "lane_change" && api_.getCurrentTime() >= 10.0) { - if (static_cast(lanechange_frames - 1) * 0.05 == 20.0) { + if (const double step_time = 50e-3, expected_time = 20.0, + real_time = static_cast(lanechange_frames) * step_time; + expected_time - step_time <= real_time && real_time <= expected_time + step_time) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp index 583d956ba9b..3d815d19113 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp @@ -47,7 +47,9 @@ class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode lanechange_frames++; } if (api_.getCurrentAction("ego") != "lane_change" && api_.getCurrentTime() >= 10.0) { - if (static_cast(lanechange_frames) * 0.05 == 20.0) { + if (const double step_time = 50e-3, expected_time = 20.0, + real_time = static_cast(lanechange_frames) * step_time; + expected_time - step_time <= real_time && real_time <= expected_time + step_time) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { stop(cpp_mock_scenarios::Result::FAILURE); diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 74363a536c3..aa3aa381327 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package openscenario_experimental_catalog ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into fix/rtc_command_action/continuous_execution +* Merge pull request `#1099 `_ from tier4/pzyskowski/660/ss2-awsim-connection +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* sample awsim scenario added, model3d added to sample vehicle catalogue +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index e81c539d1ad..6fb6d51a397 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 0.8.0 + 1.0.0 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 00e28e2b741..56da701a7a9 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -2,6 +2,148 @@ Changelog for package openscenario_interpreter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge pull request `#1077 `_ from tier4/fix/autoware-shutdown + Fix/autoware shutdown +* Fix to avoid redeclaring already declared parameter `initialize_duration` +* Add new member function `StatusMonitor::overrideThreshold` +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Masaya Kataoka, Paweł Lech, Tatsuya Yamasaki, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge pull request `#1126 `_ from tier4/fix/duplicated_nodes +* feat(user_defined_value): remove unused functional header +* feat(user_defined_value): add magic subscription counter +* feat(user_defined_value): add hash value based on name, rule, value to the subscription node name +* Merge branch 'master' into fix/duplicated_nodes +* Merge pull request `#1111 `_ from tier4/feature/traffic_light_confidence +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#1121 `_ from tier4/fix/sign-of-relative-distance +* Add comments about the historical background of `RelativeDistanceCondition`'s behavior +* Fix some `RelativeDistanceCondition` mode to return non-negative value +* chore: apply linter +* refactor: renamed 'setTrafficLightConfidence' to 'setConventionalTrafficLightConfidence' +* feat(openscenario_interpreter): add PseudoTrafficSignalDetectorConfidenceSetAction@v1 +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#997 `_ from tier4/feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into fix/rtc_command_action/continuous_execution +* Merge pull request `#1092 `_ from tier4/feature/control_rtc_auto_mode +* Merge pull request `#1098 `_ from tier4/fix/port_document +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* feat: add error message for featureIdentifiersRequiringExternalPermissionForAutonomousDecisions +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* change default port to 5555 +* Merge remote-tracking branch 'origin/master' into fix/port_document +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* refactor(simulator_core): use featureIdentifiersRequiringExternalPermissionForAutonomousDecisions instead of manualModules +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1075 `_ from tier4/feature/RJD-96_detail_message_scenario_failure +* change default port to 8080 +* ref(openscenario_interpreter): apply clang format +* Merge remote-tracking branch 'origin' into feature/RJD-96_detail_message_scenario_failure +* ref(openscenario_interpreter): ref SpecialAction +* change port +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge branch 'master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge pull request `#1095 `_ from tier4/feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* code refactor +* Merge branch 'master' into feature/freespace-distance-condition +* Implement getBoundingBox* functions +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* code refactor +* code refactor +* implement freespace for relative distance condition +* Init working version of DistanceCondition freespace +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1090 `_ from tier4/refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* feat(openscenario_interpreter): parse manualModules +* refactor: delete cooperator +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge pull request `#1083 `_ from tier4/fix/distance-condition +* Replace `LaneletId` with `lanelet::Id` +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/drop_galactic_support +* Merge pull request `#1027 `_ from tier4/feature/new_traffic_light +* ref(openscenario_interpreter): detailed result msg - last improve +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* ref(openscenario_interpreter): revert missing comment +* ref(openscenario_interpreter): revert comments format +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* ref(openscenario_interpreter): detailed result msg - improvements after feedback - third step +* Initial version of freespace distance condition +* remove workbound for galactic +* ref(openscenario_interpreter): detailed result msg - improvements after feedback - second step +* ref(openscenario_interpreter): detailed result msg - improvements after feedback - first step +* refactor: TrafficLightDetectorEmulator => PseudoTrafficLightDetector +* fix: cast result to double in order to correctly compare equals values +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* ref(scenario_failure): remove blank lines +* ref(scenario_failure): remove blank lines +* ref(openscenario_interpreter): improve ScenarioFailure +* feat(openscenario_interpreter): inherit ScenarioFailure and tidy up +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* ref(openscenario_interpreter): improve detailed failure message +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* ref(openscenario_interpreter): improve ScenarioFailure +* feat(openscenario_interpreter): mv ScenarioError, add catch InitActions +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* feat(openscenario_interpreter): attach traffic light detector emulator in applyAssignControllerAction +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* feat(openscenario_interpreter): improve exception for detail result msg +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* feat(openscenario_interpreter): first step detail result msg +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Add the property `allowGoalModification` to switch destination topics +* Remove debug prints that were left unintentionally +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into ref/RJD-553_restore_repeated_update_entity_status diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index d70a747ae5a..f7d78f99913 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 0.8.0 + 1.0.0 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 919150c5655..ee603287634 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #define DECLARE_PARAMETER(IDENTIFIER) \ declare_parameter(#IDENTIFIER, IDENTIFIER) @@ -293,7 +294,26 @@ auto Interpreter::reset() -> void publisher_of_context->on_deactivate(); } - SimulatorCore::deactivate(); + if (not has_parameter("initialize_duration")) { + declare_parameter("initialize_duration", 30); + } + + /* + Although we have not analyzed the details yet, the process of deactivating + the simulator core takes quite a long time (especially the + traffic_simulator::API::despawnEntities function is slow). During the + process, the interpreter becomes unresponsive, which often resulted in the + status monitor thread judging the interpreter as "not good". Therefore, we + will increase the threshold of the judgment only during the process of + deactivating the simulator core. + + The threshold value here is set to the value of initialize_duration, but + there is no rationale for this; it should be larger than the original + threshold value of the status monitor and long enough for the simulator + core to be deactivated. + */ + common::status_monitor.overrideThreshold( + std::chrono::seconds(get_parameter("initialize_duration").as_int()), SimulatorCore::deactivate); scenarios.pop_front(); diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 1c56c2d8ec1..ade85542845 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package openscenario_interpreter_example ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 7c9f3c6d4b9..d53a7fa7bf5 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 0.8.0 + 1.0.0 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 ef59b8fb772..451ecc3bc63 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package openscenario_interpreter_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 7a5c6d50736..e6fe9761283 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 0.8.0 + 1.0.0 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 284a151fa47..5933b6d1ad9 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package openscenario_preprocessor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge pull request `#1126 `_ from tier4/fix/duplicated_nodes +* fix(launch): remove name keyword from openscenario_interpreter, openscenario_prepreocessor and simples_sensor_simulator nodes +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 43b8f46cab1..cec656693cd 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor - 0.8.0 + 1.0.0 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 6c29bc976f2..083d2105b1e 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package openscenario_preprocessor_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 6d0f31370b5..138098ac558 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 0.8.0 + 1.0.0 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 898ea33b73d..1e6bdab54b6 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package openscenario_utility ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index c6bc12645e5..655d57c5123 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 0.8.0 + 1.0.0 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_visualization/CHANGELOG.rst b/openscenario/openscenario_visualization/CHANGELOG.rst index 9e2abceb50d..2d54310a75a 100644 --- a/openscenario/openscenario_visualization/CHANGELOG.rst +++ b/openscenario/openscenario_visualization/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package openscenario_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Merge remote-tracking branch 'origin/master' into feature/traffic-lights-awsim-support +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge pull request `#1033 `_ from tier4/feat/condition_groups_visualization +* modify comment +* add doxygen comment +* add doxygen comment +* add comment +* Refactor condition group display and event processing +* remove unnamed_event_counter +* rename condition_groups_msg to condition_groups +* added explanation for value scale +* Added the doxygen comment for ScopedPixelBuffer class +* Reformatted for better readability. +* Update openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp +* Update openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp +* Update openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp +* Update openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.hpp +* Update openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.hpp +* Update openscenario/openscenario_visualization/plugins.xml +* fix code style +* add plugin for visualize the condition results +* add jsk_overlay_utils +* update plugins.xml +* update package.xml +* update CMakeLists +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Piotr Zyskowski, kyoichi-sugahara, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/openscenario/openscenario_visualization/CMakeLists.txt b/openscenario/openscenario_visualization/CMakeLists.txt index 27c70b45498..c58057ffdbe 100644 --- a/openscenario/openscenario_visualization/CMakeLists.txt +++ b/openscenario/openscenario_visualization/CMakeLists.txt @@ -17,121 +17,61 @@ endif() # find dependencies find_package(Eigen3 REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(class_loader REQUIRED) -find_package(openscenario_interpreter_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(quaternion_operation REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rviz_common REQUIRED) -find_package(traffic_simulator REQUIRED) -find_package(traffic_simulator_msgs REQUIRED) -find_package(visualization_msgs REQUIRED) - -ament_export_dependencies(class_loader REQUIRED) -ament_export_dependencies(pluginlib REQUIRED) -ament_export_dependencies(rclcpp REQUIRED) -ament_export_dependencies(std_msgs REQUIRED) -ament_export_dependencies(Eigen3 REQUIRED) - -include_directories( - include +find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Qt settings +set(CMAKE_INCLUDE_CURRENT_DIR ON) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) +add_definitions(-DQT_NO_KEYWORDS) + +# Include directories +include_directories(include ${EIGEN3_INCLUDE_DIR} -) + ${Qt5Core_INCLUDE_DIRS} + ${Qt5Widgets_INCLUDE_DIRS} + SYSTEM + ${OGRE_INCLUDE_DIRS}) -add_library(openscenario_visualization_component SHARED +ament_auto_add_library(openscenario_visualization_component SHARED src/openscenario_visualization_component.cpp ) -ament_target_dependencies(openscenario_visualization_component - rclcpp - rclcpp_components - rclcpp_lifecycle - traffic_simulator - visualization_msgs - traffic_simulator_msgs - quaternion_operation -) -add_executable(openscenario_visualization_node + +ament_auto_add_executable(openscenario_visualization_node src/openscenario_visualization_node.cpp ) target_link_libraries(openscenario_visualization_node openscenario_visualization_component) -ament_target_dependencies(openscenario_visualization_node - quaternion_operation - rclcpp - rclcpp_components - rclcpp_lifecycle - traffic_simulator - traffic_simulator_msgs - visualization_msgs -) -############### -# Qt settings # -############### -find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) -macro(qt_wrap_ui) - qt5_wrap_ui(${ARGN}) -endmacro() -include_directories(${Qt5Core_INCLUDE_DIRS}) -include_directories(${Qt5Widgets_INCLUDE_DIRS}) add_definitions(-DQT_NO_KEYWORDS) -set(CMAKE_INCLUDE_CURRENT_DIR ON) -set(CMAKE_AUTOMOC ON) -set(CMAKE_AUTORCC ON) - -pluginlib_export_plugin_description_file(rviz_common plugins.xml) -include_directories( - include -) - -qt5_wrap_ui(UIC_FILES - src/ui/context_panel_plugin.ui -) +qt5_wrap_ui(UIC_FILES src/ui/context_panel_plugin.ui) -add_library(openscenario_visualization_rviz_plugin SHARED +ament_auto_add_library(openscenario_visualization_rviz_plugin SHARED include/openscenario_visualization/context_panel_plugin.hpp src/context_panel_plugin.cpp - ${UIC_FILES}) -ament_target_dependencies(openscenario_visualization_rviz_plugin - Qt5 - pluginlib - rclcpp - openscenario_interpreter_msgs - rviz_common + ${UIC_FILES} + include/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.hpp + src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp + include/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.hpp + src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp ) + target_include_directories(openscenario_visualization_rviz_plugin PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY icons DESTINATION share/openscenario_visualization) install(DIRECTORY include/ DESTINATION include) -install(TARGETS openscenario_visualization_rviz_plugin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -install(TARGETS openscenario_visualization_node DESTINATION lib/openscenario_visualization) - -install(TARGETS - openscenario_visualization_component - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) ament_export_libraries(openscenario_visualization_component) -rclcpp_components_register_nodes(openscenario_visualization_component - "openscenario_visualization::OpenscenarioVisualizationComponent") + +rclcpp_components_register_nodes(openscenario_visualization_component "openscenario_visualization::OpenscenarioVisualizationComponent") if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() -ament_package() +pluginlib_export_plugin_description_file(rviz_common plugins.xml) + +ament_auto_package() diff --git a/openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.hpp b/openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.hpp new file mode 100644 index 00000000000..699e57aaf64 --- /dev/null +++ b/openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.hpp @@ -0,0 +1,192 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +/** + * @class ScopedPixelBuffer + * @brief Manages a scoped pixel buffer for RViz overlay objects. + * + * This class is designed to manage the lifetime of an Ogre::HardwarePixelBufferSharedPtr. + * It locks the pixel buffer on creation and unlocks it on destruction, providing a + * convenient way to manipulate pixel buffers within a scoped lifetime. It also offers + * methods to convert the buffer into a QImage format. + */ +class ScopedPixelBuffer +{ +public: + /** + * @brief Constructs a ScopedPixelBuffer object and locks the pixel buffer. + * @param pixel_buffer The Ogre hardware pixel buffer to be managed. + */ + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + + /** + * @brief Destructor that unlocks the pixel buffer. + */ + virtual ~ScopedPixelBuffer(); + + /** + * @brief Gets the underlying Ogre hardware pixel buffer. + * @return Shared pointer to the Ogre::HardwarePixelBuffer. + */ + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + + /** + * @brief Converts the pixel buffer to a QImage. + * @param width Width of the QImage. + * @param height Height of the QImage. + * @return QImage representation of the pixel buffer. + */ + virtual QImage getQImage(unsigned int width, unsigned int height); + + /** + * @brief Converts the pixel buffer of an overlay object to a QImage. + * @param overlay Reference to the OverlayObject. + * @return QImage representation of the overlay object's pixel buffer. + */ + virtual QImage getQImage(OverlayObject & overlay); + + /** + * @brief Converts the pixel buffer to a QImage with a specified background color. + * @param width Width of the QImage. + * @param height Height of the QImage. + * @param bg_color Background color for the QImage. + * @return QImage representation of the pixel buffer with background color. + */ + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + + /** + * @brief Converts the pixel buffer of an overlay object to a QImage with a specified background color. + * @param overlay Reference to the OverlayObject. + * @param bg_color Background color for the QImage. + * @return QImage representation of the overlay object's pixel buffer with background color. + */ + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr + pixel_buffer_; ///< Shared pointer to the pixel buffer managed by this class. + +private: +}; + +/** + * @brief This is a class for put overlay object on rviz 3D panel. + * This class suppose to be instantiated in onInitialize method of rviz::Display class. + */ +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject( + Ogre::SceneManager * manager, const rclcpp::Logger & logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + virtual void hide(); + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + const rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.hpp b/openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.hpp new file mode 100644 index 00000000000..0f020511a10 --- /dev/null +++ b/openscenario/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.hpp @@ -0,0 +1,111 @@ +// 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_VISUALIZATION_CONDITION_GROUPS_HPP_ +#define OPENSCENARIO_VISUALIZATION_CONDITION_GROUPS_HPP_ + +#include + +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "jsk_overlay_utils.hpp" +#endif + +namespace openscenario_visualization +{ +struct Condition +{ + std::string current_evaluation; + std::string current_value; + std::string type; +}; + +struct ConditionGroup +{ + std::vector conditions; +}; + +struct ConditionGroups +{ + std::string groups_name; + std::vector condition_groups; +}; + +using nlohmann::json; +using openscenario_interpreter_msgs::msg::Context; +using ConditionGroupsCollection = std::vector; + +class VisualizationConditionGroupsDisplay : public rviz_common::Display +{ + Q_OBJECT + +public: + VisualizationConditionGroupsDisplay(); + virtual ~VisualizationConditionGroupsDisplay(); + + void onInitialize() override; + void onDisable() override; + void onEnable() override; + void subscribe(); + void unsubscribe(); + +private Q_SLOTS: + void updateTopic(); + void updateVisualization(); + +protected: + void processMessage(const Context::ConstSharedPtr msg_ptr); + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_length_; + rviz_common::properties::IntProperty * property_width_; + rviz_common::properties::StringProperty * property_topic_name_; + rviz_common::properties::FloatProperty * property_value_scale_; + +private: + void loadConditionGroups(const Context::ConstSharedPtr msg_ptr); + void processStory(const YAML::Node & story); + void processManeuver(const YAML::Node & maneuver); + void processEvent(const YAML::Node & event); + rclcpp::Subscription::SharedPtr simulation_context_sub_; + Context::ConstSharedPtr last_msg_ptr_; + std::shared_ptr condition_groups_collection_ptr_; +}; + +} // namespace openscenario_visualization + +#endif // OPENSCENARIO_VISUALIZATION_CONDITION_GROUPS_HPP_ diff --git a/openscenario/openscenario_visualization/package.xml b/openscenario/openscenario_visualization/package.xml index 268ff698463..3811d741a4b 100644 --- a/openscenario/openscenario_visualization/package.xml +++ b/openscenario/openscenario_visualization/package.xml @@ -2,12 +2,14 @@ openscenario_visualization - 0.8.0 + 1.0.0 Visualization tools for simulation results Masaya Kataoka + Kyoichi Sugahara Apache License 2.0 ament_cmake + ament_cmake_auto pkg-config pluginlib @@ -22,6 +24,7 @@ traffic_simulator traffic_simulator_msgs visualization_msgs + nlohmann-json-dev ament_lint_auto ament_cmake_clang_format diff --git a/openscenario/openscenario_visualization/plugins.xml b/openscenario/openscenario_visualization/plugins.xml index 7b3133e2045..a140f9b2971 100644 --- a/openscenario/openscenario_visualization/plugins.xml +++ b/openscenario/openscenario_visualization/plugins.xml @@ -4,4 +4,7 @@ Context Panel plugin. + + Display Condition Groups + diff --git a/openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp b/openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp new file mode 100644 index 00000000000..ce544b634dd --- /dev/null +++ b/openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp @@ -0,0 +1,203 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() { pixel_buffer_->unlock(); } + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() { return pixel_buffer_; } + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject( + Ogre::SceneManager * manager, const rclcpp::Logger & logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); +} + +std::string OverlayObject::getName() { return name_; } + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() { return static_cast(texture_); } + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) { panel_->setPosition(left, top); } + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() { return overlay_->isVisible(); } + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp b/openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp new file mode 100644 index 00000000000..b702b772f68 --- /dev/null +++ b/openscenario/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp @@ -0,0 +1,296 @@ +// 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 "openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace openscenario_visualization +{ +VisualizationConditionGroupsDisplay::VisualizationConditionGroupsDisplay() +: condition_groups_collection_ptr_(std::make_shared>()) +{ + /// @note Get screen info of default display + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + /// @note Fixed height for a 4k resolution screen + /// @sa https://github.com/tier4/scenario_simulator_v2/pull/1033#discussion_r1412781103 + constexpr float hight_4k = 2160.0; + + /// @note Calculate scale based on current screen height and 4k height + const float scale = static_cast(screen_info->height) / hight_4k; + + /** + * @note The multiplication of 35.0 is used to determine the initial text size in the panel. + * This value is a base size that is then scaled according to the screen resolution. + * The 'Value Scale' property allows users to adjust the scaling factor of this text size later on, + * but the initial value of 35.0 is set to ensure a default size that is likely suitable for most screens. + * The scaling factor adjusts this size to ensure readability across various resolutions. + */ + const float text_size = scale * 35.0; + + /// @note Define initial value of left edge position of condition results panel + const int left = 0; + + /** + * @note Define initial value of top edge position of condition results panel + * The multiplication of 450 sets the initial top edge position of the condition results panel. + * Like the width and length, this is a predefined base value, not necessarily linked to any value set from the Top property. + * The purpose of this calculation is to position the top edge of the panel at an appropriate place on the screen, + * again scaling according to screen resolution to maintain a consistent look across different devices. + */ + const int top = static_cast(std::round(450 * scale)); + + /** + * @note Define initial value of horizontal length of condition results panel. + * The reason 2000 is hard-coded here is because that number displayed most beautifully when we tested the operation on a 4K/non 4K display. + * Also, this number can be set via the rviz GUI. + */ + const int length = static_cast(std::round(2000 * scale)); + + /** + * @note Define initial value of width of condition results panel. + * The reason 2000 is hard-coded here is because that number displayed most beautifully when we tested the operation on a 4K/non 4K display. + * Also, this number can be set via the rviz GUI. + */ + const int width = static_cast(std::round(2000 * scale)); + + property_topic_name_ = new rviz_common::properties::StringProperty( + "Topic", "/simulation/context", "The topic on which to publish simulation context.", this, + SLOT(updateTopic()), this); + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(255, 255, 255), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_length_ = new rviz_common::properties::IntProperty( + "Length", length, "Length of the plotter window", this, SLOT(updateVisualization()), this); + property_length_->setMin(10); + property_width_ = new rviz_common::properties::IntProperty( + "Width", width, "Width of the plotter window", this, SLOT(updateVisualization()), this); + property_width_->setMin(10); + property_value_scale_ = new rviz_common::properties::FloatProperty( + "Value Scale", text_size, + "This property controls the scaling factor for the text size on the panel. Setting a higher " + "value results in larger text, making the displayed information easier to read.", + this, SLOT(updateVisualization()), this); + property_value_scale_->setMin(0.01); +} + +VisualizationConditionGroupsDisplay::~VisualizationConditionGroupsDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void VisualizationConditionGroupsDisplay::onInitialize() +{ + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "VisualizationConditionGroupsDisplayObject" << count++; + + overlay_.reset(new jsk_rviz_plugins::OverlayObject( + scene_manager_, context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(), + ss.str())); + overlay_->show(); + + overlay_->updateTextureSize(property_width_->getInt(), property_length_->getInt()); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); + updateVisualization(); +} + +void VisualizationConditionGroupsDisplay::updateTopic() +{ + unsubscribe(); + subscribe(); +} + +void VisualizationConditionGroupsDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void VisualizationConditionGroupsDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void VisualizationConditionGroupsDisplay::subscribe() +{ + std::string topic_name = property_topic_name_->getStdString(); + if (topic_name.length() > 0 && topic_name != "/") { + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + simulation_context_sub_ = raw_node->create_subscription( + topic_name, rclcpp::QoS{1}.transient_local(), + std::bind(&VisualizationConditionGroupsDisplay::processMessage, this, std::placeholders::_1)); + } +} + +void VisualizationConditionGroupsDisplay::unsubscribe() { simulation_context_sub_.reset(); } + +void VisualizationConditionGroupsDisplay::processMessage(const Context::ConstSharedPtr msg_ptr) +{ + if (!overlay_->isVisible()) return; + + // Create a QImage and fill it with transparent color. + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + // QColor text_color = property_text_color_->getColor(); + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(std::max(static_cast(property_value_scale_->getFloat()), 1)); + font.setBold(true); + painter.setFont(font); + + loadConditionGroups(msg_ptr); + + std::ostringstream context_ss; + for (const auto & condition_groups : *condition_groups_collection_ptr_) { + context_ss << std::fixed << std::setprecision(0) + << "Condition Groups Name: " << condition_groups.groups_name << std::endl; + for (const auto & condition_group : condition_groups.condition_groups) { + for (const auto & condition : condition_group.conditions) { + context_ss << " Current Evaluation: " << condition.current_evaluation << std::endl + << " Current Value: " << condition.current_value << std::endl + << " Type: " << condition.type << std::endl; + } + } + context_ss << std::endl; + } + + painter.drawText( + property_left_->getInt(), property_top_->getInt(), overlay_->getTextureWidth(), + overlay_->getTextureHeight(), Qt::AlignLeft | Qt::AlignTop, context_ss.str().c_str()); + + painter.end(); + last_msg_ptr_ = msg_ptr; +} + +void VisualizationConditionGroupsDisplay::updateVisualization() +{ + int width = property_width_->getInt(); + int height = property_length_->getInt(); + + overlay_->updateTextureSize(width, height); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(width, height); + + if (last_msg_ptr_) { + processMessage(last_msg_ptr_); + } +} + +void VisualizationConditionGroupsDisplay::loadConditionGroups(const Context::ConstSharedPtr msg_ptr) +{ + if (!msg_ptr) return; + + YAML::Node data; + try { + data = YAML::Load(msg_ptr->data); + } catch (const std::exception & e) { + throw std::runtime_error(std::string("Failed to load YAML: ") + e.what()); + } + + condition_groups_collection_ptr_->clear(); + + auto stories = data["OpenSCENARIO"]["Storyboard"]["Story"]; + for (const auto & story : stories) { + processStory(story); + } +} + +void VisualizationConditionGroupsDisplay::processStory(const YAML::Node & story_node) +{ + for (const auto & act : story_node["Act"]) { + for (const auto & maneuver_group : act["ManeuverGroup"]) { + for (const auto & maneuver : maneuver_group["Maneuver"]) { + processManeuver(maneuver); + } + } + } +} + +void VisualizationConditionGroupsDisplay::processManeuver(const YAML::Node & maneuver_node) +{ + for (const auto & event : maneuver_node["Event"]) { + if (event["StartTrigger"] && event["StartTrigger"]["ConditionGroup"]) { + processEvent(event); + } + } +} + +void VisualizationConditionGroupsDisplay::processEvent(const YAML::Node & event_node) +{ + std::string event_name; + try { + event_name = event_node["name"].as(); + } catch (const YAML::BadConversion & e) { + event_name = ""; + } + if (event_name.empty()) { + event_name = "name is not defined"; + } + + ConditionGroups condition_groups; + condition_groups.groups_name = event_name; + + for (const auto & condition_group_node : event_node["StartTrigger"]["ConditionGroup"]) { + ConditionGroup condition_group; + + for (const auto & condition_node : condition_group_node["Condition"]) { + Condition condition_msg; + condition_msg.current_evaluation = condition_node["currentEvaluation"].as(); + condition_msg.current_value = condition_node["currentValue"].as(); + condition_msg.type = condition_node["type"].as(); + + condition_group.conditions.push_back(condition_msg); + } + + condition_groups.condition_groups.push_back(condition_group); + } + + condition_groups_collection_ptr_->push_back(condition_groups); +} + +} // namespace openscenario_visualization + +#include +PLUGINLIB_EXPORT_CLASS( + openscenario_visualization::VisualizationConditionGroupsDisplay, rviz_common::Display) diff --git a/poetry.lock b/poetry.lock index 26ff99eb926..437db6f0462 100644 --- a/poetry.lock +++ b/poetry.lock @@ -1,4 +1,4 @@ -# This file is automatically @generated by Poetry 1.6.1 and should not be changed by hand. +# This file is automatically @generated by Poetry 1.7.1 and should not be changed by hand. [[package]] name = "babel" @@ -177,20 +177,20 @@ smmap = ">=3.0.1,<6" [[package]] name = "gitpython" -version = "3.1.37" +version = "3.1.41" description = "GitPython is a Python library used to interact with Git repositories" optional = false python-versions = ">=3.7" files = [ - {file = "GitPython-3.1.37-py3-none-any.whl", hash = "sha256:5f4c4187de49616d710a77e98ddf17b4782060a1788df441846bddefbb89ab33"}, - {file = "GitPython-3.1.37.tar.gz", hash = "sha256:f9b9ddc0761c125d5780eab2d64be4873fc6817c2899cbcb34b02344bdc7bc54"}, + {file = "GitPython-3.1.41-py3-none-any.whl", hash = "sha256:c36b6634d069b3f719610175020a9aed919421c87552185b085e04fbbdb10b7c"}, + {file = "GitPython-3.1.41.tar.gz", hash = "sha256:ed66e624884f76df22c8e16066d567aaa5a37d5b5fa19db2c6df6f7156db9048"}, ] [package.dependencies] gitdb = ">=4.0.1,<5" [package.extras] -test = ["black", "coverage[toml]", "ddt (>=1.1.1,!=1.4.3)", "mypy", "pre-commit", "pytest", "pytest-cov", "pytest-sugar"] +test = ["black", "coverage[toml]", "ddt (>=1.1.1,!=1.4.3)", "mock", "mypy", "pre-commit", "pytest (>=7.3.1)", "pytest-cov", "pytest-instafail", "pytest-mock", "pytest-sugar", "sumtypes"] [[package]] name = "httplib2" @@ -219,13 +219,13 @@ files = [ [[package]] name = "jinja2" -version = "3.1.2" +version = "3.1.3" description = "A very fast and expressive template engine." optional = false python-versions = ">=3.7" files = [ - {file = "Jinja2-3.1.2-py3-none-any.whl", hash = "sha256:6088930bfe239f0e6710546ab9c19c9ef35e29792895fed6e6e31a023a182a61"}, - {file = "Jinja2-3.1.2.tar.gz", hash = "sha256:31351a702a408a9e7595a8fc6150fc3f43bb6bf7e319770cbc0db9df9437e852"}, + {file = "Jinja2-3.1.3-py3-none-any.whl", hash = "sha256:7d6d50dd97d52cbc355597bd845fabfbac3f551e1f99619e39a35ce8c370b5fa"}, + {file = "Jinja2-3.1.3.tar.gz", hash = "sha256:ac8bd6544d4bb2c9792bf3a159e80bba8fda7f07e81bc3aed565432d5925ba90"}, ] [package.dependencies] diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst new file mode 100644 index 00000000000..61b64d29397 --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -0,0 +1,21 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package real_time_factor_control_rviz_plugin +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.0 (2024-02-14) +------------------ +* Merge pull request `#1185 `_ from tier4/feature/new_release_flow + Feature/new release flow +* change package version +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge branch 'master' into fix/autoware-shutdown +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/consider_tread_in_ego_entity +* Merge pull request `#1150 `_ from tier4/feature/real-time-factor-control + Feature/real time factor control +* New line at the EOF +* Changes after review +* Removed autoware_cmake dependency from real time factor control rviz plugin +* Setting concealer use_sim_time manually instead of using global arguments. +* Corrected time storage in Simulation Clock. Other minor changes +* RViz plugin controlling real time factor value +* Contributors: Kotaro Yoshimoto, Masaya Kataoka, Paweł Lech, Tatsuya Yamasaki diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt b/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt new file mode 100644 index 00000000000..978867f719b --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) + +project(real_time_factor_control_rviz_plugin) + +find_package(ament_cmake_auto REQUIRED) +find_package(Qt5 REQUIRED Core Widgets) + +ament_auto_find_build_dependencies() + +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +ament_auto_add_library(${PROJECT_NAME} SHARED src/real_time_factor_slider.cpp) + +target_compile_definitions(${PROJECT_NAME} PUBLIC QT_NO_KEYWORDS) + +target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) + +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES}) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package() diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml new file mode 100644 index 00000000000..f3efb3735cb --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -0,0 +1,28 @@ + + + + real_time_factor_control_rviz_plugin + 1.0.0 + Slider controlling real time factor value. + Paweł Lech + Apache License 2.0 + + Paweł Lech + + ament_cmake + ament_cmake_auto + + libqt5-core + libqt5-widgets + qtbase5-dev + rviz_common + std_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 00000000000..59119575da2 --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,7 @@ + + + Slider controlling real time factor value. + + diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp new file mode 100644 index 00000000000..1d4aa9bd4cd --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.cpp @@ -0,0 +1,64 @@ +// +// Copyright 2020 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 + +namespace real_time_factor_control_rviz_plugin +{ +RealTimeFactorSliderPanel::RealTimeFactorSliderPanel(QWidget * parent) +: rviz_common::Panel(parent), + value_label_(new QLabel("x 1.00")), + slider_(new QSlider(Qt::Horizontal)) +{ + value_label_->setAlignment(Qt::AlignCenter); + + slider_->setMinimum(1); + slider_->setMaximum(200); + slider_->setTickInterval(1); + slider_->setValue(100); + + auto layout = new QHBoxLayout(this); + layout->addWidget(value_label_); + layout->addWidget(slider_); + + setLayout(layout); +} + +auto RealTimeFactorSliderPanel::onChangedRealTimeFactorValue(int percentage) -> void +{ + std_msgs::msg::Float64 real_time_factor; + real_time_factor.data = percentage / 100.0; + real_time_factor_publisher->publish(real_time_factor); + value_label_->setText(QString("x ") + QString::number(real_time_factor.data, 'f', 2)); +} + +auto RealTimeFactorSliderPanel::onInitialize() -> void +{ + real_time_factor_publisher = getDisplayContext() + ->getRosNodeAbstraction() + .lock() + ->get_raw_node() + ->create_publisher("/real_time_factor", 1); + + connect(slider_, SIGNAL(valueChanged(int)), SLOT(onChangedRealTimeFactorValue(int))); +} +} // namespace real_time_factor_control_rviz_plugin + +PLUGINLIB_EXPORT_CLASS( + real_time_factor_control_rviz_plugin::RealTimeFactorSliderPanel, rviz_common::Panel) diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp new file mode 100644 index 00000000000..671b349604e --- /dev/null +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/src/real_time_factor_slider.hpp @@ -0,0 +1,56 @@ +// +// Copyright 2023 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 REAL_TIME_FACTOR_SLIDER_PANEL_HPP_ +#define REAL_TIME_FACTOR_SLIDER_PANEL_HPP_ + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include +#include +#endif + +namespace real_time_factor_control_rviz_plugin +{ +class RealTimeFactorSliderPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit RealTimeFactorSliderPanel(QWidget * parent = nullptr); + + auto onInitialize() -> void override; + +public Q_SLOTS: + /* + Declaring this function by trailing return type causes Qt AutoMoc + subprocess error. + */ + void onChangedRealTimeFactorValue(int real_time_factor_value); + +protected: + rclcpp::Publisher::SharedPtr real_time_factor_publisher; + + QLabel * const value_label_; + + QSlider * const slider_; +}; +} // namespace real_time_factor_control_rviz_plugin + +#endif // REAL_TIME_FACTOR_SLIDER_PANEL_HPP_ diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 69fc6232a5f..29dcb1ca08f 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package scenario_simulator_v2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index c49ab6f3033..83fa1ab8ab6 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 0.8.0 + 1.0.0 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 b0b1f912f32..831d0f0d856 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -2,6 +2,89 @@ Changelog for package behavior_tree_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into prepare/release-0.9.0 +* Merge pull request `#1129 `_ from tier4/feature/RJD-716_add_follow_waypoint_controller +* feat(follow_trajectory): add target_speed into consideration +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge branch 'master' into fix/duplicated_nodes +* Merge pull request `#1111 `_ from tier4/feature/traffic_light_confidence +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* Merge remote-tracking branch 'origin/master' into feature/deleted-entity +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#1104 `_ from tier4/fix/entities-right-of-way +* chore: suppress warning from boost library +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1093 `_ from tier4/feature/RJD-614_follow_trajectory_action_pedestrian_cyclist_support +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* fix(action_node): calculate the backward distance in getYieldStopDistance +* fix(action_node): check if is not the same right of way for two diffrent lanelets +* Merge remote-tracking branch 'origin/master' into fix/port_document +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1019 `_ from tier4/feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1090 `_ from tier4/refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* feat(behavior_tree): add FollowPolyline action to pedestrian +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge pull request `#1069 `_ from tier4/feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Replace `std::vector` with `lanelet::Ids` +* Replace `std::int64_t` with `lanelet::Id` +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* remove workbound for galactic +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Update `makeUpdatedStatus` to take a reference to `PolylineTrajectory` instead of a pointer +* fix compile error +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* fix compile error +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 8ef75d001f4..e94bbba4f3d 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 0.8.0 + 1.0.0 Behavior tree plugin for traffic_simulator masaya Apache 2.0 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 1e53f0e9dd0..b3a38ec0d37 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 @@ -65,7 +65,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus } else if ( const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, step_time)) { + behavior_parameter, step_time, target_speed)) { setOutput( "updated_status", std::make_shared( 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 f571b3a9ada..6b164b7214f 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 @@ -65,7 +65,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus } else if ( const auto updated_status = traffic_simulator::follow_trajectory::makeUpdatedStatus( static_cast(*entity_status), *polyline_trajectory, - behavior_parameter, step_time)) { + behavior_parameter, step_time, target_speed)) { setOutput( "updated_status", std::make_shared( diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index 03c8bf90415..b491f73fe37 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -2,6 +2,61 @@ Changelog for package do_nothing_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge branch 'master' into fix/duplicated_nodes +* Merge pull request `#1111 `_ from tier4/feature/traffic_light_confidence +* chore: suppress warning from boost library +* Merge remote-tracking branch 'origin' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1090 `_ from tier4/refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Replace `std::vector` with `lanelet::Ids` +* Replace `std::int64_t` with `lanelet::Id` +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* remove workbound for galactic +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 821518d758c..dadc7ed6f75 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 0.8.0 + 1.0.0 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 6ef4f6851f6..ec8ae4d4469 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -2,6 +2,122 @@ Changelog for package simple_sensor_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge pull request `#1184 `_ from tier4/feature/consider_tread_in_ego_entity + Feature/consider tread in ego entity +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge branch 'master' into fix/autoware-shutdown +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/consider_tread_in_ego_entity +* Merge pull request `#1150 `_ from tier4/feature/real-time-factor-control + Feature/real time factor control +* add setTwist/setAcceleration function +* extend matching length +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Setting concealer use_sim_time manually instead of using global arguments. +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* UpdateStepTime request for updating simple sensor simulation step_time +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Kotaro Yoshimoto, Masaya Kataoka, Paweł Lech, Tatsuya Yamasaki, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge remote-tracking branch 'origin/master' into feature/traffic-lights-awsim-support +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge pull request `#1144 `_ from tier4/feature/update_sim_model +* doc: change comment style with note commands +* doc: fix note command place +* chore(simple_sensor_simulator): fix indent +* chore(simple_sensor_simulator): use @note command +* chore(simple_sensor_simulator): delete acceleration_map.csv +* feat(simple_sensor_simulator): import sim_model_delay_steer_map_acc_geared +* feat(simple_sensor_simulator): import steer dead band feature +* refactor: reflect the reviews +* refactor(simple_sensor_simulator): delete name of unused argument to suppress warnings +* change default port to 5555 +* change default port to 8080 +* change port +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge pull request `#1069 `_ from tier4/feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/drop_galactic_support +* Merge pull request `#1027 `_ from tier4/feature/new_traffic_light +* refactor(traffic_simulator): change to a comparison method that is resistant to version changes +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* refactor(simple_sensor_simulator): add explicit keyword to constructor +* refactor(simple_sensor_simulator): TrafficLightDetectorEmulator => PseudoTrafficLightDetector +* refactor(simple_sensor_simulator): TrafficLightDetectorEmulator => PseudoTrafficLightDetector +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* remove workbound for galactic +* refactor: TrafficLightDetectorEmulator => PseudoTrafficLightDetector +* chore: change architecture_type to awf/universe/20230906 +* fix: build errors +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Update `update` to use `updateStatus` instead of `setStatus` +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Update the Ego entity's FollowTrajectoryAction to take into account the vehicle model type +* Update yaw calculation during `FollowTrajectoryAction` for Ego entities +* Rename argument `time` to `current_scenario_time` +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/ref/RJD-553_restore_repeated_update_entity_status' into pzyskowski/660/ss2-awsim-connection +* Update `EgoEntitySimulation::update` to follow trajectory if is given +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* refactor: apply formatter +* fix: build errors +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Update `followPolylineTrajectory` to store the given trajectory to ego +* Simplify member function `ScenarioSimulator::isEgo` +* Simplify member function `ScenarioSimulator::isEntityExists` +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* refactor: update architecture_type format +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* fix(simple_sensor_simulator): update topic name for conventional traffic light +* refactor(simple_sensor_simulator): delete debug code +* refactor(simple_sensor_simulator): move include +* feat: add new architecture_type awf/universe/2023.08 +* refactor: delete unused lines/files +* refactor(simple_sensor_simulator): delete TrafficLightsDetectorBase +* chore: apply formatter +* feat(simple_sensor_simulator): implement ScenarioSimulator::attachTrafficLightDetectorEmulator +* refactor(simple_sensor_simulator): use TrafficLightPublisher in simple_sensor_simulator +* refactor(simple_sensor_simulator): use UpdateTrafficLightsRequest as a type for storing +* feat(simple_sensor_simulator): add base class for TrafficLightsDetector +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Revert "WIP: get and sets for vehicle status" +* WIP: get and sets for vehicle status +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge pull request `#1058 `_ from tier4/ref/RJD-553_restore_repeated_update_entity_status 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 7c9dfb300f9..6927a364d4f 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 @@ -92,7 +92,7 @@ class SensorSimulation } auto attachPseudoTrafficLightsDetector( - const double current_simulation_time, + const double /*current_simulation_time*/, const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration, rclcpp::Node & node, std::shared_ptr hdmap_utils) -> void { 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 5edd84e8a29..8547427f2ff 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 @@ -96,6 +96,9 @@ class ScenarioSimulator : public rclcpp::Node auto updateFrame(const simulation_api_schema::UpdateFrameRequest &) -> simulation_api_schema::UpdateFrameResponse; + auto updateStepTime(const simulation_api_schema::UpdateStepTimeRequest &) + -> simulation_api_schema::UpdateStepTimeResponse; + auto updateEntityStatus(const simulation_api_schema::UpdateEntityStatusRequest &) -> simulation_api_schema::UpdateEntityStatusResponse; 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 3d306a67f29..e281439e26d 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 @@ -62,6 +62,7 @@ class EgoEntitySimulation public: const std::shared_ptr hdmap_utils_ptr_; + const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; private: auto getCurrentPose() const -> geometry_msgs::msg::Pose; @@ -79,7 +80,7 @@ class EgoEntitySimulation explicit EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters &, double, - const std::shared_ptr &); + const std::shared_ptr &, const rclcpp::Parameter & use_sim_time); auto update(double time, double step_time, bool npc_logic_started) -> void; diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index f1bdba391a2..1fd45b5f81c 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 0.8.0 + 1.0.0 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index aafab6ef288..798c9871a25 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -47,7 +47,8 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options) [this](auto &&... xs) { return followPolylineTrajectory(std::forward(xs)...); }, [this](auto &&... xs) { return attachPseudoTrafficLightDetector(std::forward(xs)...); - }) + }, + [this](auto &&... xs) { return updateStepTime(std::forward(xs)...); }) { } @@ -133,6 +134,15 @@ auto ScenarioSimulator::updateFrame(const simulation_api_schema::UpdateFrameRequ return res; } +auto ScenarioSimulator::updateStepTime(const simulation_api_schema::UpdateStepTimeRequest & req) + -> simulation_api_schema::UpdateStepTimeResponse +{ + auto res = simulation_api_schema::UpdateStepTimeResponse(); + step_time_ = req.simulation_step_time(); + res.mutable_result()->set_success(true); + return res; +} + auto ScenarioSimulator::updateEntityStatus( const simulation_api_schema::UpdateEntityStatusRequest & req) -> simulation_api_schema::UpdateEntityStatusResponse @@ -198,7 +208,8 @@ auto ScenarioSimulator::spawnVehicleEntity( traffic_simulator_msgs::msg::VehicleParameters parameters; simulation_interface::toMsg(req.parameters(), parameters); ego_entity_simulation_ = std::make_shared( - parameters, step_time_, hdmap_utils_); + parameters, step_time_, hdmap_utils_, + get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false))); traffic_simulator_msgs::msg::EntityStatus initial_status; initial_status.name = parameters.name; simulation_interface::toMsg(req.pose(), initial_status.pose); 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 428ca770686..a184d40cc55 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 @@ -34,12 +34,15 @@ static auto getParameter(const std::string & name, T value = {}) EgoEntitySimulation::EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time, - const std::shared_ptr & hdmap_utils) + const std::shared_ptr & hdmap_utils, + const rclcpp::Parameter & use_sim_time) : autoware(std::make_unique()), vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), - hdmap_utils_ptr_(hdmap_utils) + hdmap_utils_ptr_(hdmap_utils), + vehicle_parameters(parameters) { + autoware->set_parameter(use_sim_time); } auto toString(const VehicleModelType datum) -> std::string @@ -395,12 +398,23 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets()); std::optional lanelet_pose; + const auto get_matching_length = [&] { + return std::max( + vehicle_parameters.axles.front_axle.track_width, + vehicle_parameters.axles.rear_axle.track_width) * + 0.5 + + 1.0; + }; + if (unique_route_lanelets.empty()) { - lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, 1.0); + lanelet_pose = hdmap_utils_ptr_->toLaneletPose( + status.pose, status.bounding_box, false, get_matching_length()); } else { - lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, 1.0); + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, get_matching_length()); if (!lanelet_pose) { - lanelet_pose = hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, 1.0); + lanelet_pose = hdmap_utils_ptr_->toLaneletPose( + status.pose, status.bounding_box, false, get_matching_length()); } } if (lanelet_pose) { diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 5d83f2ff98e..d14b765cf67 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -2,6 +2,76 @@ Changelog for package simulation_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge branch 'master' into fix/autoware-shutdown +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/consider_tread_in_ego_entity +* Merge pull request `#1150 `_ from tier4/feature/real-time-factor-control + Feature/real time factor control +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* fix format +* add const to all functions in hdmap utils class +* UpdateStepTime request for updating simple sensor simulation step_time +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Kotaro Yoshimoto, Masaya Kataoka, Paweł Lech, Tatsuya Yamasaki, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/drop_galactic_support +* Merge pull request `#1027 `_ from tier4/feature/new_traffic_light +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* refactor(simulation_interface) +* refactor(simulation_interface) +* Update simulation/simulation_interface/include/simulation_interface/conversions.hpp +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* chore: apply formatter +* refactor: TrafficLightDetectorEmulator => PseudoTrafficLightDetector +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* refactor: apply formatter +* fix: build errors +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* refactor: delete unused code / dependencies +* refactor(simulation_interface): delete unused conversion +* refactor(traffic_simulator): refactor data flow with simulation_api_schema::TrafficSignal +* refactor(simulation_interface/zmq_client): remove duplicated traffic light detector emulator call +* feat(simulator_interface): add support for autoware_msgs +* feat(simulator_interface): add AttachTrafficLightDetectorEmulatorRequest to ZMQ client/server +* chore(traffic_simulator): reformat +* feat(simlation_interface): add call function for AttachTrafficLightDetectorEmulatorRequest +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* feat(simulation_interface): attachTrafficLightDetectorEmulatorRequest/Response +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge pull request `#1058 `_ from tier4/ref/RJD-553_restore_repeated_update_entity_status 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 b33463f09b6..0d14b562aab 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp +++ b/simulation/simulation_interface/include/simulation_interface/zmq_multi_client.hpp @@ -49,6 +49,9 @@ class MultiClient auto call(const simulation_api_schema::UpdateFrameRequest &) -> simulation_api_schema::UpdateFrameResponse; + auto call(const simulation_api_schema::UpdateStepTimeRequest &) + -> simulation_api_schema::UpdateStepTimeResponse; + auto call(const simulation_api_schema::SpawnVehicleEntityRequest &) -> simulation_api_schema::SpawnVehicleEntityResponse; 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 0eb17135ffd..97e4f24f8c5 100644 --- a/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp +++ b/simulation/simulation_interface/include/simulation_interface/zmq_multi_server.hpp @@ -73,6 +73,7 @@ class MultiServer DEFINE_FUNCTION_TYPE(UpdateTrafficLights); DEFINE_FUNCTION_TYPE(FollowPolylineTrajectory); DEFINE_FUNCTION_TYPE(AttachPseudoTrafficLightDetector); + DEFINE_FUNCTION_TYPE(UpdateStepTime); #undef DEFINE_FUNCTION_TYPE @@ -80,7 +81,7 @@ class MultiServer Initialize, UpdateFrame, SpawnVehicleEntity, SpawnPedestrianEntity, SpawnMiscObjectEntity, DespawnEntity, UpdateEntityStatus, AttachLidarSensor, AttachDetectionSensor, AttachOccupancyGridSensor, UpdateTrafficLights, FollowPolylineTrajectory, - AttachPseudoTrafficLightDetector> + AttachPseudoTrafficLightDetector, UpdateStepTime> functions_; }; } // namespace zeromq diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 03653d9dbea..7bb16e54509 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 0.8.0 + 1.0.0 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 5fe4135087f..7f1c3f80d87 100644 --- a/simulation/simulation_interface/proto/simulation_api_schema.proto +++ b/simulation/simulation_interface/proto/simulation_api_schema.proto @@ -324,6 +324,20 @@ message FollowPolylineTrajectoryResponse { Result result = 1; } +/** + * Requests updating simulation step time. + **/ +message UpdateStepTimeRequest { + double simulation_step_time = 1; +} + +/** + * Response of updating simulation step time. + **/ +message UpdateStepTimeResponse { + Result result = 1; // Result of [UpdateStepTimeRequest](#UpdateStepTimeRequest) +} + /** * Universal message for Request **/ @@ -342,6 +356,7 @@ message SimulationRequest { UpdateTrafficLightsRequest update_traffic_lights = 11; FollowPolylineTrajectoryRequest follow_polyline_trajectory = 12; AttachPseudoTrafficLightDetectorRequest attach_pseudo_traffic_light_detector = 13; + UpdateStepTimeRequest update_step_time = 14; } } @@ -364,5 +379,6 @@ message SimulationResponse { UpdateTrafficLightsResponse update_traffic_lights = 11; FollowPolylineTrajectoryResponse follow_polyline_trajectory = 12; AttachPseudoTrafficLightDetectorResponse attach_pseudo_traffic_light_detector = 13; + UpdateStepTimeResponse update_step_time = 14; } } diff --git a/simulation/simulation_interface/src/zmq_multi_client.cpp b/simulation/simulation_interface/src/zmq_multi_client.cpp index ef339d1fa26..32483feaa86 100644 --- a/simulation/simulation_interface/src/zmq_multi_client.cpp +++ b/simulation/simulation_interface/src/zmq_multi_client.cpp @@ -74,6 +74,18 @@ auto MultiClient::call(const simulation_api_schema::UpdateFrameRequest & request } } +auto MultiClient::call(const simulation_api_schema::UpdateStepTimeRequest & request) + -> simulation_api_schema::UpdateStepTimeResponse +{ + if (is_running) { + simulation_api_schema::SimulationRequest sim_request; + *sim_request.mutable_update_step_time() = request; + return call(sim_request).update_step_time(); + } else { + return {}; + } +} + auto MultiClient::call(const simulation_api_schema::SpawnVehicleEntityRequest & request) -> simulation_api_schema::SpawnVehicleEntityResponse { diff --git a/simulation/simulation_interface/src/zmq_multi_server.cpp b/simulation/simulation_interface/src/zmq_multi_server.cpp index 4b59f731e1d..eb4c8211857 100644 --- a/simulation/simulation_interface/src/zmq_multi_server.cpp +++ b/simulation/simulation_interface/src/zmq_multi_server.cpp @@ -82,6 +82,10 @@ void MultiServer::poll() std::get(functions_)( proto.attach_pseudo_traffic_light_detector()); break; + case simulation_api_schema::SimulationRequest::RequestCase::kUpdateStepTime: + *sim_response.mutable_update_step_time() = + std::get(functions_)(proto.update_step_time()); + break; case simulation_api_schema::SimulationRequest::RequestCase::REQUEST_NOT_SET: { THROW_SIMULATION_ERROR("No case defined for oneof in SimulationRequest message"); } diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index 30f9397e13b..61e924f0685 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -2,6 +2,309 @@ Changelog for package traffic_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge pull request `#1184 `_ from tier4/feature/consider_tread_in_ego_entity + Feature/consider tread in ego entity +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge branch 'master' into fix/autoware-shutdown +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/consider_tread_in_ego_entity +* Merge pull request `#1150 `_ from tier4/feature/real-time-factor-control + Feature/real time factor control +* Doxygen note +* modify rviz settings +* remove externally_updated_status\_ +* add setTwist/setAcceleration function +* add setMapPose function +* Merge remote-tracking branch 'tier/master' into feature/real-time-factor-control +* Merge pull request `#1180 `_ from tier4/add_dynamic_obstacle_stop_markers + Add debug and virtual wall markers for dynamic_obstacle_avoidance +* fix indent +* Add debug and virtual wall markers for dynamic_obstacle_avoidance +* Merge remote-tracking branch 'tier/master' into feature/real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge pull request `#1159 `_ from tier4/revert/1096 + Revert/1096 +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into revert/1096 +* Changes after review +* Merge pull request `#1168 `_ from tier4/feature/get_stop_line_ids + Feature/get stop line ids +* clean up code +* rename from "Reg Elements" to "Regulatory Elements" +* use push_back +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* use emplace_back +* use emplace_back +* use emplace_back +* reformat +* apply reformat +* fix getNextLaneletIds function +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* update traffic light manager class +* add getStopLines function +* add function +* Merge pull request `#1154 `_ from tier4/cleanup/add_const_to_hdmap_utils + Cleanup/add const to hdmap utils +* use_sim_time for openscenario_interpreter is parameterized and False by default +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Revert "feat: add deleted entity to traffic simulator" + This reverts commit ba2abf393757a53e266476fc7f4184cf495837af. +* Revert "feat: remove DELETED entity type by using internal id" + This reverts commit a15268f290e4957fbcfce1e3c52c37de23852a4c. +* Revert "feat: invalidate status in deleted entity" + This reverts commit b35981583909f9ddfe9587a6ea92239a7324418e. +* remove & from function arguments with lanelet::Id type +* Corrected time storage in Simulation Clock. Other minor changes +* add const to all functions in hdmap utils class +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* RViz plugin controlling real time factor value +* UpdateStepTime request for updating simple sensor simulation step_time +* Possibility of changing SimulationClock::realtime_factor during the simulation with ROS 2 topic +* realtime factor fix +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Kotaro Yoshimoto, Masaya Kataoka, Maxime CLEMENT, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into prepare/release-0.9.0 +* Merge pull request `#1129 `_ from tier4/feature/RJD-716_add_follow_waypoint_controller +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Merge pull request `#1149 `_ from tier4/feature/traffic-lights-awsim-support +* Merge remote-tracking branch 'origin/master' into feature/traffic-lights-awsim-support +* ref(follow_trajectory): revert change to ensure non-void return +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* fix(follow_trajectory): fix target_speed, little refactor +* ref(follow_trajectory): apply review changes - patch +* Merge pull request `#1033 `_ from tier4/feat/condition_groups_visualization +* clang format +* two topics publishing v2i traffic lights +* Merge remote-tracking branch 'origin/master' into feature/traffic-lights-awsim-support +* Reformatted for better readability. +* feat(traffic_simulator): add EntityManager::setTrafficLightConfidence +* update config +* update rviz config +* update rviz config +* Merge pull request `#1145 `_ from tier4/feature/random_scenario +* feat(follow_trajectory): add target_speed into consideration +* fix(follow_trajectory): remove unecessary auto +* fix(follow_waypoint_controller): fix for value type +* fix(follow_waypoint_controller): fix lambda return +* fix(spell_check): fix comment +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* feat(follow_waypoint_controller): review changes +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* cheanged default v2i traffic lights topic +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Merge pull request `#1137 `_ from tier4/fix/RJD-727_fix_orientation_for_inactivity +* fix(follow_trajectory): fix orientation for norm(v)==0 +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* ref(follow_trajectory): remove unecessary changes +* fix(follow_trajectory): fix distance precision and add exception details +* fix(follow_waypoint_controller): fix arrival tolerance +* fix(follow_waypoint_controller): fix finish tolerance +* fix(follow_waypoint_controller): add copyright +* fix(follow_waypoint_controller): fix warnings +* fix(follow_waypoint_controller): fix warnings +* feat(follow_trajectory): add follow waypoint controller +* Merge branch 'master' into fix/duplicated_nodes +* Merge pull request `#1111 `_ from tier4/feature/traffic_light_confidence +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* fix: modify comment in traffic_light.hpp +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* fix: build errors +* Merge pull request `#1113 `_ from tier4/feature/doxygen +* Merge remote-tracking branch 'origin/master' into fix/sign-of-relative-distance +* Merge pull request `#1096 `_ from tier4/feature/deleted-entity +* Update simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +* Update simulation/traffic_simulator/include/traffic_simulator/data_type/speed_change.hpp +* Update simulation/traffic_simulator/include/traffic_simulator/data_type/speed_change.hpp +* Update simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +* add clang-format off +* refactor: reflect the reviews +* update doxyfile +* add simulation package +* add comment +* add spawn outside vehicle +* refactor: renamed 'setTrafficLightConfidence' to 'setConventionalTrafficLightConfidence' +* update Doxyfile +* fix(traffic_simulator): update setTrafficLightConfidence function to handle multiple TrafficLights +* fix(traffic_simulator): fix build errors +* feat(openscenario_interpreter): add PseudoTrafficSignalDetectorConfidenceSetAction@v1 +* feat(traffic_simulator): add EntityManager::setTrafficLightConfidence +* feat(traffic_simulator): move confidence field from Bulb class to TrafficLight class +* feat: invalidate status in deleted entity +* feat: remove DELETED entity type by using internal id +* feat: add deleted entity to traffic simulator +* feat(traffic_simulator): add confidence to Bulb class +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into fix/rtc_command_action/continuous_execution +* Merge pull request `#1092 `_ from tier4/feature/control_rtc_auto_mode +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* chore: apply clang_format +* feat(traffic_simualtor): set use_foa=true to use default auto_mode settings +* Merge pull request `#1101 `_ from tier4/fix/standstill_duration +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1093 `_ from tier4/feature/RJD-614_follow_trajectory_action_pedestrian_cyclist_support +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* add updateStandStillDuration(step_time) and updateTraveledDistance(step_time) when the entity failed to match lanelet +* remove debug lines +* Merge remote-tracking branch 'origin' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge branch 'master' into AJD-805/baseline_update_rebased +* enable clean up entity +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge pull request `#1095 `_ from tier4/feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Implement getBoundingBox* functions +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* code refactor +* fix spelling +* implement freespace for relative distance condition +* Init working version of DistanceCondition freespace +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1090 `_ from tier4/refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* feat(behavior_tree): add FollowPolyline action to pedestrian +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge pull request `#1069 `_ from tier4/feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* remove debug line +* fix case +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Rename `gelAllCanonicalizedLaneletPoses` to `get...` +* Cleanup +* Replace `std::vector` with `lanelet::Ids` +* Replace `std::int64_t` with `lanelet::Id` +* Replace `LaneletId` with `lanelet::Id` +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/drop_galactic_support +* Merge pull request `#1027 `_ from tier4/feature/new_traffic_light +* chore: apply formatter +* refactor(HdMapUtils): rename functions related to traffic light +* refactor(traffic_simulator): change to a comparison method that is resistant to version changes +* refactor(traffic_simulator): delete unnecessary optimization +* fix(traffic_simulator): fix build errors +* chore(traffic_simulator): add LaneletId alias +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* refactor: use better word "thunk" instead of callback +* refactor(traffic_simulator): use LaneletId instead of std::int64_t +* refactor(traffic_simulator) +* refactor(traffic_simulator) +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* apply reformat +* Initial version of freespace distance condition +* remove workbound for galactic +* chore: apply formatter +* refactor: TrafficLightDetectorEmulator => PseudoTrafficLightDetector +* chore: change architecture_type to awf/universe/20230906 +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* apply reformat +* refactor: apply formatter with clang-format v14 +* reintroduced clock publshing +* add getSValue function +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* add white space +* apply reformat +* commented clock and parts of concealer +* refactor: apply formatter +* refactor: delete debug messages +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* chore: apply formatter +* fix(TrafficLight): delete relation_id in TrafficLight class and use latest getTrafficLightRelationIDFromWayID +* fix(HDMapUtils): return all relation ids from getTrafficLightRelationIDFromWayID +* apply reformat +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Update `makeUpdatedStatus` to take a reference to `PolylineTrajectory` instead of a pointer +* apply reformat +* fix(traffic_simulator/ego): switch a parameter for new architecture_type +* fix compile error +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* refactor: delete unused code / dependencies +* feat: add new architecture_type awf/universe/2023.08 +* chore: apply formatter +* refactor: delete unused lines/files +* feat(traffic_simulator): implement conversion from way_id to relation_id +* refactor(traffic_simulator): fix initialization order of member variables in ConfigurableRateUpdater +* chore: apply formatter +* fix(traffic_simulator): fix compile errors +* refactor(traffic_simulator): refactor data flow with simulation_api_schema::TrafficSignal +* refactor(traffic_simulator): refactor ConfigurableRateUpdater +* refactor(traffic_simulator): delete V2ITrafficLightPublisher and use TrafficLightPublisher +* feat(traffic_simulator): use new ConfigurableRateUpdater in TrafficLightMarkerPublisher +* feat(traffic_simulator): implement proto exporting from TrafficLightManager +* feat(traffic_simulator): Add TrafficLightPublisher +* refactor(traffic_simulator): refactor ConfigurableRateUpdater +* refactor(traffic_simulator): delete unused files +* feat(simple_sensor_simulator): add base class for TrafficLightsDetector +* fix(traffic_simulator): fix build errors +* chore(traffic_simulator): reformat +* fix(traffic_simulator): use new message in V2ITrafficLightPublisher +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* fix(traffic_simulator): fix V2ITrafficLightPublisher +* feat(simulation_interface): attachTrafficLightDetectorEmulatorRequest/Response +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* chore(traffic_light): apply ament_clang_format +* refactor(traffic_light): use new architecture_type +* chore(traffic_light): delay creating publisher +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* chore(traffic_light): override pure virtual function +* chore(traffic_light): delete empty files +* chore(traffic_light): adapt to variations of traffic light bulb types +* fix(entity_manager): fix errors for traffic light tests +* feat(entity_manager): use new traffic light managers in entity_manager +* feat(traffic_light): add codes for new message +* chore(traffic_light): prepare for runtime topic adaptation +* chore(traffic_light): delete TrafficLightManager type specialization +* chore(traffic_light): rename timers +* chore(traffic_light): set relation_id for new message type +* chore(traffic_light): add template for TrafficLight::Bulb conversion operator +* chore(traffic_light): fix errors +* fix(HDMapUtils): getTrafficLightRelationIDFromWayID +* chore: reformat +* feat(HDMapUtils): getTrafficLightRelationIDFromWayID +* chore(traffic_light): add perception messages for traffic light +* refactor(traffic_light): use TrafficLightBase +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* refactor(traffic_light): use updated function name +* feat(traffic_light): add TrafficLightBase and parameterize msg type of TrafficLight +* refactor(HDMapUtil): rename some functions +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, f0reachARR, kyoichi-sugahara, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge pull request `#1058 `_ from tier4/ref/RJD-553_restore_repeated_update_entity_status diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index a9471765aa9..122d3bd60a9 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -29,6 +29,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(traffic_simulator SHARED src/api/api.cpp src/behavior/follow_trajectory.cpp + src/behavior/follow_waypoint_controller.cpp src/behavior/longitudinal_speed_planning.cpp src/behavior/route_planner.cpp src/color_utils/color_utils.cpp @@ -37,7 +38,6 @@ ament_auto_add_library(traffic_simulator SHARED src/data_type/lane_change.cpp src/data_type/lanelet_pose.cpp src/data_type/speed_change.cpp - src/entity/deleted_entity.cpp src/entity/ego_entity.cpp src/entity/entity_base.cpp src/entity/entity_manager.cpp diff --git a/simulation/traffic_simulator/config/scenario_simulator_v2.rviz b/simulation/traffic_simulator/config/scenario_simulator_v2.rviz index b2897138ae6..72a64ca063a 100644 --- a/simulation/traffic_simulator/config/scenario_simulator_v2.rviz +++ b/simulation/traffic_simulator/config/scenario_simulator_v2.rviz @@ -2096,6 +2096,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_drivable_lane Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DynamicObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/dynamic_obstacle_stop + Value: true Enabled: true Name: VirtualWall - Class: rviz_common/Group @@ -2378,6 +2390,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_drivable_lane Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DynamicObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/dynamic_obstacle_stop + Value: false Enabled: false Name: DebugMarker - Class: rviz_common/Group @@ -2952,6 +2976,22 @@ Visualization Manager: Reliability Policy: Reliable Value: /simulation/traffic_light/marker Value: true + - Class: openscenario_visualization/VisualizationConditionGroups + Name: Condition Group + Topic: /simulation/context + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Debug Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /simulation/debug_marker + Value: false - Class: rviz_common/Group Displays: - Alpha: 0.9990000128746033 @@ -3056,7 +3096,7 @@ Visualization Manager: Value: /perception/occupancy_grid_map/map_updates Use Timestamp: false Value: false - Enabled: false + Enabled: true Name: Simple Sensor Simulator Enabled: false Name: Simulation diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index b66ec2b09a7..86843200f1d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -74,7 +75,21 @@ class API rclcpp::PublisherOptionsWithAllocator())), debug_marker_pub_(rclcpp::create_publisher( node, "debug_marker", rclcpp::QoS(100), rclcpp::PublisherOptionsWithAllocator())), - clock_(std::forward(xs)...), + real_time_factor_subscriber(rclcpp::create_subscription( + node, "/real_time_factor", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort(), + [this](const std_msgs::msg::Float64 & message) { + /** + * @note Pausing the simulation by setting the realtime_factor_ value to 0 is not supported and causes the simulation crash. + * For that reason, before performing the action, it needs to be ensured that the incoming request data is a positive number. + */ + if (message.data >= 0.001) { + clock_.realtime_factor = message.data; + simulation_api_schema::UpdateStepTimeRequest request; + request.set_simulation_step_time(clock_.getStepTime()); + zeromq_client_.call(request); + } + })), + clock_(node->get_parameter("use_sim_time").as_bool(), std::forward(xs)...), zeromq_client_( simulation_interface::protocol, configuration.simulator_host, getZMQSocketPort(*node)) { @@ -333,6 +348,7 @@ class API FORWARD_TO_ENTITY_MANAGER(requestWalkStraight); FORWARD_TO_ENTITY_MANAGER(resetConventionalTrafficLightPublishRate); FORWARD_TO_ENTITY_MANAGER(resetV2ITrafficLightPublishRate); + FORWARD_TO_ENTITY_MANAGER(setAcceleration); FORWARD_TO_ENTITY_MANAGER(setAccelerationLimit); FORWARD_TO_ENTITY_MANAGER(setAccelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setBehaviorParameter); @@ -340,6 +356,8 @@ class API FORWARD_TO_ENTITY_MANAGER(setDecelerationLimit); FORWARD_TO_ENTITY_MANAGER(setDecelerationRateLimit); FORWARD_TO_ENTITY_MANAGER(setLinearVelocity); + FORWARD_TO_ENTITY_MANAGER(setMapPose); + FORWARD_TO_ENTITY_MANAGER(setTwist); FORWARD_TO_ENTITY_MANAGER(setVelocityLimit); FORWARD_TO_ENTITY_MANAGER(toMapPose); @@ -370,6 +388,8 @@ class API const rclcpp::Publisher::SharedPtr debug_marker_pub_; + const rclcpp::Subscription::SharedPtr real_time_factor_subscriber; + SimulationClock clock_; zeromq::MultiClient zeromq_client_; 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 36aa391c19f..e979358d28f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp @@ -27,7 +27,8 @@ namespace follow_trajectory auto makeUpdatedStatus( const traffic_simulator_msgs::msg::EntityStatus &, traffic_simulator_msgs::msg::PolylineTrajectory &, - const traffic_simulator_msgs::msg::BehaviorParameter &, double step_time) + const traffic_simulator_msgs::msg::BehaviorParameter &, double step_time, + std::optional target_speed = std::nullopt) -> std::optional; } // namespace follow_trajectory } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp new file mode 100644 index 00000000000..1a4154d13c5 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp @@ -0,0 +1,300 @@ +// Copyright 2023 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 TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ +struct ControllerError : public common::Error +{ + template + explicit ControllerError(Ts &&... xs) + : common::Error(common::concatenate( + "An error occurred in the internal controller operation in the FollowTrajectoryAction. ", + "Please report the following information to the developer: ", + std::forward(xs)...)) + { + } +}; + +struct PredictedState +{ + double acceleration; + double speed; + double traveled_distance; + double travel_time; + + auto moveStraight(const double step_acceleration, const double step_time) -> void + { + acceleration = step_acceleration; + speed += acceleration * step_time; + traveled_distance += speed * step_time; + travel_time += step_time; + } + + auto isImmobile(const double tolerance) const + { + return std::abs(speed) < tolerance && std::abs(acceleration) < tolerance; + } + + template + friend auto operator<<(StreamType & stream, const PredictedState & state) -> StreamType & + { + stream << std::setprecision(16) << std::fixed; + stream << "PredictedState: acceleration: " << state.acceleration << ", speed: " << state.speed + << ", distance: " << state.traveled_distance << ", time: " << state.travel_time << ". "; + return stream; + } +}; + +class FollowWaypointController +{ + const double step_time; + const bool with_breaking; + + const double max_speed; + const double max_acceleration; + const double max_acceleration_rate; + const double max_deceleration; + const double max_deceleration_rate; + + const double target_speed; + + /* + Achieving official epsilon (1e-16) accuracy when using doubles is + difficult for this reason the controller uses less accuracy. + + There is no technical basis for this value, it was determined based on + Dawid Moszynski experiments. + */ + static constexpr double local_epsilon = 1e-12; + + /* + Acceptable time step inaccuracy, allowing the time to be rounded up to the + full number of steps. + + There is no technical basis for this value, it was determined based on + Dawid Moszynski experiments. + */ + static constexpr double step_time_tolerance = 1e-6; + + /* + Accuracy of the final arrival distance at a waypoint with a specified + time. + + There is no technical basis for this value, it was determined based on + Dawid Moszynski experiments. + */ + static constexpr double finish_distance_tolerance = 1e-4; + + /* + Accuracy of the predicted arrival distance at the waypoint with the + specified time it is only used to detect in advance that it is most likely + impossible to arrive at a sufficient final accuracy. + + There is no technical basis for this value, it was determined based on + Dawid Moszynski experiments. + */ + static constexpr double predicted_distance_tolerance = 1.0; + + /* + Number of considered acceleration candidates. This is a discretization of + the current range of [min_acceleration, max_acceleration]. + + There is no technical basis for this value, it was determined based on + Dawid Moszynski experiments. + */ + static constexpr std::size_t number_of_acceleration_candidates = 20; + + /* + This is a debugging method, it is not worth giving it much attention. + */ + template + friend auto operator<<(StreamType & stream, const FollowWaypointController & c) -> StreamType & + { + stream << std::setprecision(16) << std::fixed; + stream << "FollowWaypointController: step_time: " << c.step_time + << ", with_breaking: " << c.with_breaking << ", max_speed: " << c.max_speed + << ", max_acceleration: " << c.max_acceleration + << ", max_deceleration: " << c.max_deceleration + << ", max_acceleration_rate: " << c.max_acceleration_rate + << ", max_deceleration_rate: " << c.max_deceleration_rate + << ", target_speed: " << c.target_speed << "."; + return stream; + } + + /* + This provides an analytical method for calculating the acceleration that + will allow the best possible accuracy in arriving at the waypoint during + the last few frames. It provides the calculation of acceleration for + arriving at waypoint without and with braking. + + The method allows to calculate only the last 3 steps without braking and 4 + with braking, because analytical calculation of a greater number of steps + is difficult - a greater number of steps is processed through the + prediction of the controller. + + Details: + + - Acceleration at the last step equal to 0.0, therefore in the penultimate + step the acceleration is set to 0.0 + + - Only with braking: speed at the last and penultimate step equal to 0.0, + therefore when there are 3 steps remain the acceleration is calculated + in such a way as to set the speed to 0.0 (a=-v/t) + + - Only with braking: because the speed is set to 0.0 when there are 3 + steps remain, in the last 3 steps no distance will be traveled, + therefore, when there are 4 steps remain, it is necessary to choose the + acceleration in such a way as to travel exactly the remaining distance + in this step (s = at^2/2+vt -> a = 2(s-vt)/t^2). + + - Only without braking: in the penultimate step, the acceleration is set + to 0, therefore, when 3 steps remain it is necessary to choose the + acceleration in such a way as to ensure that in the penultimate and last + step the distance traveled is equal to the remaining distance (s = last + + penultimate = (v+at)t + at^2/2+vt -> a = 2/3(s-2vt)/t^2). + */ + auto getAnalyticalAccelerationForLastSteps( + const double remaining_time, const double remaining_distance, const double acceleration, + const double speed) const -> double; + + /* + This allows the correct counting of the remaining number of steps. + Without this, inaccuracy sometimes results in a decrease of more than 1 + step which can cause major difficulties if we want to arrive at the + waypoint at the precise time. + */ + auto roundTimeToFullStepsWithTolerance( + const double remaining_time_source, const double time_tolerance) const -> double; + + /* + This allows to calculate how much time (rounded up to whole steps) is + needed to reach acceleration equal to zero - which is necessary in case of + achieving the maximum speed (to not exceed it) and achieving the speed + equal to 0.0 (to not start moving backwards). + */ + auto getTimeRequiredForNonAcceleration(const double acceleration) const -> double; + + /* + This allows the calculation of acceleration limits that meet the + constraints. The limits depend on the current acceleration, the current + speed and the limits on the change in acceleration (rate). + */ + auto getAccelerationLimits(const double acceleration, const double speed) const + -> std::pair; + + auto clampAcceleration( + const double candidate_acceleration, const double acceleration, const double speed) const + -> double; + + auto moveStraight(PredictedState & state, const double candidate_acceleration) const -> void; + + auto getPredictedStopStateWithoutConsideringTime( + const double step_acceleration, const double remaining_distance, const double acceleration, + const double speed) const -> std::optional; + +public: + explicit constexpr FollowWaypointController( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time, const bool with_breaking, + const std::optional & target_speed = std::nullopt) + : step_time{step_time}, + with_breaking{with_breaking}, + max_speed{behavior_parameter.dynamic_constraints.max_speed}, + max_acceleration{behavior_parameter.dynamic_constraints.max_acceleration}, + max_acceleration_rate{behavior_parameter.dynamic_constraints.max_acceleration_rate}, + max_deceleration{behavior_parameter.dynamic_constraints.max_deceleration}, + max_deceleration_rate{behavior_parameter.dynamic_constraints.max_deceleration_rate}, + target_speed{(target_speed) ? target_speed.value() : max_speed} + { + } + + /* + This is a debugging method, it is not worth giving it much attention. + */ + auto getFollowedWaypointDetails( + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const + -> std::string + { + if (const auto & vertices = polyline_trajectory.shape.vertices; !vertices.empty()) { + std::stringstream waypoint_details; + waypoint_details << "Currently followed waypoint: "; + if (const auto first_waypoint_with_arrival_time_specified = std::find_if( + vertices.begin(), vertices.end(), + [](auto && vertex) { return not std::isnan(vertex.time); }); + first_waypoint_with_arrival_time_specified != + std::end(polyline_trajectory.shape.vertices)) { + waypoint_details << "[" << first_waypoint_with_arrival_time_specified->position.position.x + << ", " << first_waypoint_with_arrival_time_specified->position.position.y + << "] with specified time equal to " + << first_waypoint_with_arrival_time_specified->time << "s. "; + } else { + waypoint_details << "[" << vertices.back().position.position.x << ", " + << vertices.back().position.position.y << "] without specified time. "; + } + return waypoint_details.str(); + } else { + return "No followed waypoint - trajectory is empty."; + } + } + + /* + This allows to predict the state that will be reached if the current + acceleration is changed from acceleration to step_acceleration. + */ + auto getPredictedWaypointArrivalState( + const double step_acceleration, const double remaining_time, const double remaining_distance, + const double acceleration, const double speed) const -> std::optional; + /* + This allows the best acceleration to be found for the current conditions, + without taking into account the arrival time - this is the case when every + next point of the trajectory has no specified time. + */ + auto getAcceleration( + const double remaining_distance, const double acceleration, const double speed) const -> double; + + /* + This allows the best acceleration to be found for the current conditions, + taking into account the arrival time. + */ + auto getAcceleration( + const double remaining_time_source, const double remaining_distance, const double acceleration, + const double speed) const -> double; + + auto areConditionsOfArrivalMet( + const double acceleration, const double speed, const double distance) const -> double + { + return (!with_breaking || std::abs(speed) < local_epsilon) && + std::abs(acceleration) < local_epsilon && distance < finish_distance_tolerance; + } +}; +} // namespace follow_trajectory +} // namespace traffic_simulator + +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/deleted_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/deleted_entity.hpp deleted file mode 100644 index d2290469cd7..00000000000 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/deleted_entity.hpp +++ /dev/null @@ -1,107 +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. - -#ifndef TRAFFIC_SIMULATOR__ENTITY__DELETED_ENTITY_HPP_ -#define TRAFFIC_SIMULATOR__ENTITY__DELETED_ENTITY_HPP_ - -#include -#include -#include - -namespace traffic_simulator -{ -namespace entity -{ -class DeletedEntity : public EntityBase -{ -public: - // Dummy ID for internal use - static constexpr uint8_t ENTITY_TYPE_ID = 0xFF; - - explicit DeletedEntity( - const std::string & name, const CanonicalizedEntityStatus &, - const std::shared_ptr &); - - void onUpdate(double, double) override {} - - auto getCurrentAction() const -> std::string override; - - 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 = ENTITY_TYPE_ID; // Dummy ID for internal use - return type; - } - - auto getEntityTypename() const -> const std::string & override - { - static const std::string result = "DeletedEntity"; - return result; - } - - ~DeletedEntity() override = default; - - auto getGoalPoses() -> std::vector override { return {}; } - - std::optional getObstacle() override - { - return std::nullopt; - } - - auto getRouteLanelets(double) -> lanelet::Ids override { return {}; } - - auto fillLaneletPose(CanonicalizedEntityStatus &) -> void override; - - auto getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsArray override - { - return traffic_simulator_msgs::msg::WaypointsArray(); - } - - void requestSpeedChange(double, bool) override {} - - void requestSpeedChange(const speed_change::RelativeTargetSpeed &, bool) override {} - - void requestAssignRoute(const std::vector &) override {} - - void requestAssignRoute(const std::vector &) override {} - - void requestAcquirePosition(const CanonicalizedLaneletPose &) override {} - - void requestAcquirePosition(const geometry_msgs::msg::Pose &) override {} - - void requestSpeedChange( - const double, const speed_change::Transition, const speed_change::Constraint, - const bool) override - { - } - - auto getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter override; - - void setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) override {} - - void setAccelerationLimit(double) override {} - - void setAccelerationRateLimit(double) override {} - - void setDecelerationLimit(double) override {} - - void setDecelerationRateLimit(double) override {} -}; -} // namespace entity -} // namespace traffic_simulator - -#endif // TRAFFIC_SIMULATOR__ENTITY__DELETED_ENTITY_HPP_ 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 833a0922450..d29a5a1b330 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -39,8 +39,6 @@ class EgoEntity : public VehicleEntity static auto makeFieldOperatorApplication(const Configuration &) -> std::unique_ptr; - CanonicalizedEntityStatus externally_updated_status_; - public: explicit EgoEntity() = delete; @@ -107,8 +105,6 @@ class EgoEntity : public VehicleEntity auto setBehaviorParameter(const traffic_simulator_msgs::msg::BehaviorParameter &) -> void override; - auto setStatusExternally(const CanonicalizedEntityStatus & status) -> void; - void requestSpeedChange(double, bool continuous) override; void requestSpeedChange( @@ -116,6 +112,8 @@ class EgoEntity : public VehicleEntity auto setVelocityLimit(double) -> void override; + auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void override; + auto fillLaneletPose(CanonicalizedEntityStatus & status) -> void override; }; } // namespace entity 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 68c49c9b560..81399e55e91 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -219,6 +219,12 @@ class EntityBase virtual auto setVelocityLimit(double) -> void; + virtual auto setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void; + + /* */ auto setTwist(const geometry_msgs::msg::Twist & twist) -> void; + + /* */ auto setAcceleration(const geometry_msgs::msg::Accel & accel) -> void; + virtual void startNpcLogic(); /* */ void stopAtCurrentPosition(); 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 7d93aed4ef2..0359bb27b28 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -32,7 +32,6 @@ #include #include #include -#include #include #include #include @@ -117,6 +116,7 @@ class EntityManager const std::shared_ptr v2i_traffic_light_manager_ptr_; const std::shared_ptr v2i_traffic_light_marker_publisher_ptr_; + const std::shared_ptr v2i_traffic_light_legacy_topic_publisher_ptr_; const std::shared_ptr v2i_traffic_light_publisher_ptr_; ConfigurableRateUpdater v2i_traffic_light_updater_, conventional_traffic_light_updater_; @@ -180,14 +180,18 @@ class EntityManager v2i_traffic_light_manager_ptr_(std::make_shared(hdmap_utils_ptr_)), v2i_traffic_light_marker_publisher_ptr_( std::make_shared(v2i_traffic_light_manager_ptr_, node)), - v2i_traffic_light_publisher_ptr_( + v2i_traffic_light_legacy_topic_publisher_ptr_( makeV2ITrafficLightPublisher("/v2x/traffic_signals", node, hdmap_utils_ptr_)), + v2i_traffic_light_publisher_ptr_(makeV2ITrafficLightPublisher( + "/perception/traffic_light_recognition/external/traffic_signals", node, hdmap_utils_ptr_)), v2i_traffic_light_updater_( node, [this]() { v2i_traffic_light_marker_publisher_ptr_->publish(); v2i_traffic_light_publisher_ptr_->publish( clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); + v2i_traffic_light_legacy_topic_publisher_ptr_->publish( + clock_ptr_->now(), v2i_traffic_light_manager_ptr_->generateUpdateTrafficLightsRequest()); }), conventional_traffic_light_updater_( node, [this]() { conventional_traffic_light_marker_publisher_ptr_->publish(); }) @@ -278,7 +282,7 @@ class EntityManager // clang-format on FORWARD_TO_ENTITY(asFieldOperatorApplication, const); - FORWARD_TO_ENTITY(cancelRequest, ); + FORWARD_TO_ENTITY(fillLaneletPose, const); FORWARD_TO_ENTITY(get2DPolygon, const); FORWARD_TO_ENTITY(getBehaviorParameter, const); FORWARD_TO_ENTITY(getBoundingBox, const); @@ -293,27 +297,30 @@ class EntityManager FORWARD_TO_ENTITY(getDistanceToRightLaneBound, const); FORWARD_TO_ENTITY(getEntityStatusBeforeUpdate, const); FORWARD_TO_ENTITY(getEntityType, const); - FORWARD_TO_ENTITY(fillLaneletPose, const); FORWARD_TO_ENTITY(getLaneletPose, const); FORWARD_TO_ENTITY(getLinearJerk, const); FORWARD_TO_ENTITY(getMapPose, const); FORWARD_TO_ENTITY(getMapPoseFromRelativePose, const); - FORWARD_TO_ENTITY(getRouteLanelets, ); + 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(requestAcquirePosition, ); FORWARD_TO_ENTITY(requestAssignRoute, ); FORWARD_TO_ENTITY(requestFollowTrajectory, ); FORWARD_TO_ENTITY(requestLaneChange, ); FORWARD_TO_ENTITY(requestWalkStraight, ); - FORWARD_TO_ENTITY(activateOutOfRangeJob, ); + FORWARD_TO_ENTITY(setAcceleration, ); FORWARD_TO_ENTITY(setAccelerationLimit, ); FORWARD_TO_ENTITY(setAccelerationRateLimit, ); FORWARD_TO_ENTITY(setBehaviorParameter, ); FORWARD_TO_ENTITY(setDecelerationLimit, ); FORWARD_TO_ENTITY(setDecelerationRateLimit, ); FORWARD_TO_ENTITY(setLinearVelocity, ); + FORWARD_TO_ENTITY(setMapPose, ); + FORWARD_TO_ENTITY(setTwist, ); FORWARD_TO_ENTITY(setVelocityLimit, ); #undef FORWARD_TO_ENTITY @@ -469,9 +476,6 @@ class EntityManager auto setEntityStatus(const std::string & name, const CanonicalizedEntityStatus &) -> void; - auto setEntityStatusExternally(const std::string & name, const CanonicalizedEntityStatus &) - -> void; - void setVerbose(const bool verbose); template 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 bb6221cb03c..9c4ad92f3fe 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/pedestrian_entity.hpp @@ -119,6 +119,8 @@ class PedestrianEntity : public EntityBase const std::string plugin_name; + const traffic_simulator_msgs::msg::PedestrianParameters pedestrian_parameters; + private: pluginlib::ClassLoader loader_; const std::shared_ptr behavior_plugin_ptr_; 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 66d6e9dc71c..b7b8507e767 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/vehicle_entity.hpp @@ -122,6 +122,8 @@ class VehicleEntity : public EntityBase auto fillLaneletPose(CanonicalizedEntityStatus & status) -> void override; + const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; + private: pluginlib::ClassLoader loader_; 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 7306d3a5b74..24375e4b211 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 @@ -63,7 +63,7 @@ class HdMapUtils public: explicit HdMapUtils(const boost::filesystem::path &, const geographic_msgs::msg::GeoPoint &); - auto canChangeLane(lanelet::Id from, lanelet::Id to) const -> bool; + auto canChangeLane(const lanelet::Id from, const lanelet::Id to) const -> bool; auto canonicalizeLaneletPose(const traffic_simulator_msgs::msg::LaneletPose &) const -> std::tuple< @@ -75,8 +75,8 @@ class HdMapUtils std::optional, std::optional>; auto clipTrajectoryFromLaneletIds( - lanelet::Id, double s, const lanelet::Ids &, double forward_distance = 20) const - -> std::vector; + const lanelet::Id, const double s, const lanelet::Ids &, + const double forward_distance = 20) const -> std::vector; auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids; @@ -85,21 +85,23 @@ class HdMapUtils auto getAllCanonicalizedLaneletPoses(const traffic_simulator_msgs::msg::LaneletPose &) const -> std::vector; - auto getAlongLaneletPose(const traffic_simulator_msgs::msg::LaneletPose & from, double along) - const -> traffic_simulator_msgs::msg::LaneletPose; + auto getAlongLaneletPose( + const traffic_simulator_msgs::msg::LaneletPose & from, const double along) const + -> traffic_simulator_msgs::msg::LaneletPose; auto getCenterPoints(const lanelet::Ids &) const -> std::vector; - auto getCenterPoints(lanelet::Id) const -> std::vector; + auto getCenterPoints(const lanelet::Id) const -> std::vector; - auto getCenterPointsSpline(lanelet::Id) const + auto getCenterPointsSpline(const lanelet::Id) const -> std::shared_ptr; auto getClosestLaneletId( - const geometry_msgs::msg::Pose &, double distance_thresh = 30.0, - bool include_crosswalk = false) const -> std::optional; + const geometry_msgs::msg::Pose &, const double distance_thresh = 30.0, + const bool include_crosswalk = false) const -> std::optional; - auto getCollisionPointInLaneCoordinate(lanelet::Id, lanelet::Id crossing_lanelet_id) const + auto getCollisionPointInLaneCoordinate( + const lanelet::Id lanelet_id, const lanelet::Id crossing_lanelet_id) const -> std::optional; auto getConflictingCrosswalkIds(const lanelet::Ids &) const -> lanelet::Ids; @@ -131,10 +133,11 @@ class HdMapUtils const lanelet::Id traffic_light_id) const -> std::optional; auto getFollowingLanelets( - lanelet::Id, const lanelet::Ids & candidate_lanelet_ids, double distance = 100, - bool include_self = true) const -> lanelet::Ids; + const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, + const double distance = 100, const bool include_self = true) const -> lanelet::Ids; - auto getFollowingLanelets(lanelet::Id, double distance = 100, bool include_self = true) const + auto getFollowingLanelets( + const lanelet::Id, const double distance = 100, const bool include_self = true) const -> lanelet::Ids; auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; @@ -142,8 +145,8 @@ class HdMapUtils auto getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from, const traffic_simulator::lane_change::Parameter & lane_change_parameter, - double maximum_curvature_threshold, double target_trajectory_length, - double forward_distance_threshold) const + const double maximum_curvature_threshold, const double target_trajectory_length, + const double forward_distance_threshold) const -> std::optional>; auto getLaneChangeTrajectory( @@ -151,62 +154,65 @@ class HdMapUtils const traffic_simulator::lane_change::Parameter & lane_change_parameter) const -> std::optional>; - auto getLaneChangeableLaneletId(lanelet::Id, traffic_simulator::lane_change::Direction) const + auto getLaneChangeableLaneletId( + const lanelet::Id, const traffic_simulator::lane_change::Direction) const -> std::optional; auto getLaneChangeableLaneletId( - lanelet::Id, traffic_simulator::lane_change::Direction, std::uint8_t shift) const - -> std::optional; + const lanelet::Id, const traffic_simulator::lane_change::Direction, + const std::uint8_t shift) const -> std::optional; auto getLaneletIds() const -> lanelet::Ids; - auto getLaneletLength(lanelet::Id) const -> double; + auto getLaneletLength(const lanelet::Id) const -> double; - auto getLaneletPolygon(lanelet::Id) const -> std::vector; + auto getLaneletPolygon(const lanelet::Id) const -> std::vector; auto getLateralDistance( const traffic_simulator_msgs::msg::LaneletPose & from, const traffic_simulator_msgs::msg::LaneletPose & to) const -> std::optional; - auto getLeftBound(lanelet::Id) const -> std::vector; + auto getLeftBound(const lanelet::Id) const -> std::vector; auto getLeftLaneletIds( - lanelet::Id, traffic_simulator_msgs::msg::EntityType, - bool include_opposite_direction = true) const -> lanelet::Ids; + const lanelet::Id, const traffic_simulator_msgs::msg::EntityType &, + const bool include_opposite_direction = true) const -> lanelet::Ids; auto getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from, const traffic_simulator_msgs::msg::LaneletPose & to) const -> std::optional; auto getNearbyLaneletIds( - const geometry_msgs::msg::Point &, double distance_threshold, bool include_crosswalk, - std::size_t search_count = 5) const -> lanelet::Ids; + const geometry_msgs::msg::Point &, const double distance_threshold, + const bool include_crosswalk, const std::size_t search_count = 5) const -> lanelet::Ids; auto getNearbyLaneletIds( - const geometry_msgs::msg::Point &, double distance_threshold, - std::size_t search_count = 5) const -> lanelet::Ids; + const geometry_msgs::msg::Point &, const double distance_threshold, + const std::size_t search_count = 5) const -> lanelet::Ids; auto getNextLaneletIds(const lanelet::Ids &) const -> lanelet::Ids; auto getNextLaneletIds(const lanelet::Ids &, const std::string & turn_direction) const -> lanelet::Ids; - auto getNextLaneletIds(lanelet::Id) const -> lanelet::Ids; + auto getNextLaneletIds(const lanelet::Id) const -> lanelet::Ids; - auto getNextLaneletIds(lanelet::Id, const std::string & turn_direction) const -> lanelet::Ids; + auto getNextLaneletIds(const lanelet::Id, const std::string & turn_direction) const + -> lanelet::Ids; auto getPreviousLaneletIds(const lanelet::Ids &) const -> lanelet::Ids; auto getPreviousLaneletIds(const lanelet::Ids &, const std::string & turn_direction) const -> lanelet::Ids; - auto getPreviousLaneletIds(lanelet::Id) const -> lanelet::Ids; + auto getPreviousLaneletIds(const lanelet::Id) const -> lanelet::Ids; - auto getPreviousLaneletIds(lanelet::Id, const std::string & turn_direction) const -> lanelet::Ids; + auto getPreviousLaneletIds(const lanelet::Id, const std::string & turn_direction) const + -> lanelet::Ids; - auto getPreviousLanelets(lanelet::Id, double distance = 100) const -> lanelet::Ids; + auto getPreviousLanelets(const lanelet::Id, const double distance = 100) const -> lanelet::Ids; - auto getRightBound(lanelet::Id) const -> std::vector; + auto getRightBound(const lanelet::Id) const -> std::vector; auto getRightLaneletIds( lanelet::Id, traffic_simulator_msgs::msg::EntityType, @@ -215,19 +221,22 @@ class HdMapUtils auto getRightOfWayLaneletIds(const lanelet::Ids &) const -> std::unordered_map; - auto getRightOfWayLaneletIds(lanelet::Id) const -> lanelet::Ids; + auto getRightOfWayLaneletIds(const lanelet::Id) const -> lanelet::Ids; - auto getRoute(lanelet::Id from, lanelet::Id to) const -> lanelet::Ids; + auto getRoute(const lanelet::Id from, const lanelet::Id to) const -> lanelet::Ids; auto getSpeedLimit(const lanelet::Ids &) const -> double; + auto getStopLineIds() const -> lanelet::Ids; + auto getStopLineIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids; - auto getStopLinePolygon(lanelet::Id) const -> std::vector; + auto getStopLinePolygon(const lanelet::Id) const -> std::vector; - auto getTangentVector(lanelet::Id, double s) const -> std::optional; + auto getTangentVector(const lanelet::Id, const double s) const + -> std::optional; - auto getTrafficLightBulbPosition(lanelet::Id traffic_light_id, const std::string &) const + auto getTrafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string &) const -> std::optional; auto getTrafficLightIds() const -> lanelet::Ids; @@ -240,16 +249,16 @@ class HdMapUtils auto getTrafficLightStopLineIds(const lanelet::Id traffic_light_id) const -> lanelet::Ids; - auto getTrafficLightStopLinesPoints(lanelet::Id traffic_light_id) const + auto getTrafficLightStopLinesPoints(const lanelet::Id traffic_light_id) const -> std::vector>; auto insertMarkerArray( visualization_msgs::msg::MarkerArray &, const visualization_msgs::msg::MarkerArray &) const -> void; - auto isInLanelet(lanelet::Id, double s) const -> bool; + auto isInLanelet(const lanelet::Id, const double s) const -> bool; - auto isInRoute(lanelet::Id, const lanelet::Ids & route) const -> bool; + auto isInRoute(const lanelet::Id, const lanelet::Ids & route) const -> bool; auto isTrafficLight(const lanelet::Id) const -> bool; @@ -257,32 +266,36 @@ class HdMapUtils auto matchToLane( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - bool include_crosswalk, double reduction_ratio = 0.8) const -> std::optional; + const bool include_crosswalk, const double reduction_ratio = 0.8) const + -> std::optional; auto toLaneletPose( - const geometry_msgs::msg::Pose &, bool include_crosswalk, double matching_distance = 1.0) const + const geometry_msgs::msg::Pose &, const bool include_crosswalk, + const double matching_distance = 1.0) const -> std::optional; auto toLaneletPose( - const geometry_msgs::msg::Pose &, const lanelet::Ids &, double matching_distance = 1.0) const + const geometry_msgs::msg::Pose &, const lanelet::Ids &, + const double matching_distance = 1.0) const -> std::optional; auto toLaneletPose( const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - bool include_crosswalk, double matching_distance = 1.0) const + const bool include_crosswalk, const double matching_distance = 1.0) const -> std::optional; - auto toLaneletPose(const geometry_msgs::msg::Pose &, lanelet::Id, double matching_distance = 1.0) - const -> std::optional; + auto toLaneletPose( + const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 1.0) const + -> std::optional; auto toLaneletPoses( - const geometry_msgs::msg::Pose &, lanelet::Id, double matching_distance = 5.0, - bool include_opposite_direction = true) const + const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 5.0, + const bool include_opposite_direction = true) const -> std::vector; auto toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin; - auto toMapPoints(lanelet::Id, const std::vector & s) const + auto toMapPoints(const lanelet::Id, const std::vector & s) const -> std::vector; auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &) const @@ -342,27 +355,32 @@ class HdMapUtils auto getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from, const traffic_simulator_msgs::msg::LaneletPose & to, - const traffic_simulator::lane_change::TrajectoryShape, double tangent_vector_size = 100) const - -> math::geometry::HermiteCurve; + const traffic_simulator::lane_change::TrajectoryShape, + const double tangent_vector_size = 100) const -> math::geometry::HermiteCurve; auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets; - auto getNextRoadShoulderLanelet(lanelet::Id) const -> lanelet::Ids; + auto getNextRoadShoulderLanelet(const lanelet::Id) const -> lanelet::Ids; - auto getPreviousRoadShoulderLanelet(lanelet::Id) const -> lanelet::Ids; + auto getPreviousRoadShoulderLanelet(const lanelet::Id) const -> lanelet::Ids; + + auto getStopLines() const -> lanelet::ConstLineStrings3d; auto getStopLinesOnPath(const lanelet::Ids &) const -> lanelet::ConstLineStrings3d; - auto getTrafficLightRegElementsOnPath(const lanelet::Ids &) const + auto getTrafficLightRegulatoryElementsOnPath(const lanelet::Ids &) const -> std::vector>; auto getTrafficLights(const lanelet::Id traffic_light_id) const -> std::vector; - auto getTrafficSignRegElementsOnPath(const lanelet::Ids &) const + auto getTrafficSignRegulatoryElementsOnPath(const lanelet::Ids &) const + -> std::vector>; + + auto getTrafficSignRegulatoryElements() const -> std::vector>; - auto getVectorFromPose(const geometry_msgs::msg::Pose &, double magnitude) const + auto getVectorFromPose(const geometry_msgs::msg::Pose &, const double magnitude) const -> geometry_msgs::msg::Vector3; auto mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin &) const -> void; diff --git a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp index d6d5deaed5f..94f6642638c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/simulation_clock/simulation_clock.hpp @@ -23,36 +23,39 @@ namespace traffic_simulator class SimulationClock : rclcpp::Clock { public: - explicit SimulationClock(double realtime_factor, double frame_rate); + explicit SimulationClock(bool use_sim_time, double realtime_factor, double frame_rate); auto getCurrentRosTime() -> rclcpp::Time; auto getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock; - auto getCurrentScenarioTime() const { return (frame_ - frame_offset_) / frame_rate; } + auto getCurrentScenarioTime() const + { + return seconds_since_the_simulator_started_ - seconds_at_the_start_of_the_scenario_; + } - auto getCurrentSimulationTime() const { return frame_ / frame_rate; } + auto getCurrentSimulationTime() const { return seconds_since_the_simulator_started_; } - auto getStepTime() const { return realtime_factor / frame_rate; } + auto getStepTime() const { return realtime_factor / frame_rate_; } auto start() -> void; - auto started() const { return not std::isnan(frame_offset_); } + auto started() const { return not std::isnan(seconds_at_the_start_of_the_scenario_); } auto update() -> void; - const bool use_raw_clock; + const bool use_sim_time; double realtime_factor; - double frame_rate; +private: + double frame_rate_; - const rclcpp::Time time_on_initialize; + const rclcpp::Time time_at_the_start_of_the_simulator_; -private: - double frame_ = 0; + double seconds_since_the_simulator_started_ = 0.0; - double frame_offset_ = std::numeric_limits::quiet_NaN(); + double seconds_at_the_start_of_the_scenario_ = std::numeric_limits::quiet_NaN(); }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp index 3bb4700c45b..c43e9e940a6 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/traffic_lights/traffic_light_manager.hpp @@ -44,6 +44,8 @@ class TrafficLightManager auto getTrafficLight(const lanelet::Id traffic_light_id) -> TrafficLight &; + auto getTrafficLightIds() const -> const lanelet::Ids; + auto getTrafficLights() const -> const TrafficLightMap &; auto getTrafficLights() -> TrafficLightMap &; diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index a8378211064..e64149889f6 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 0.8.0 + 1.0.0 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 133147d84e1..9f530e7007b 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -237,13 +237,9 @@ bool API::updateEntitiesStatusInSim() simulation_interface::toMsg(res_status.action_status(), entity_status.action_status); if (entity_manager_ptr_->isEgo(name)) { - // temporarily deinitialize lanelet pose as it should be correctly filled from here - entity_status.lanelet_pose_valid = false; - entity_status.lanelet_pose = traffic_simulator_msgs::msg::LaneletPose(); - auto canonicalized = canonicalize(entity_status); - /// @note apply additional status data (from ll2) and then set status - entity_manager_ptr_->fillLaneletPose(name, canonicalized); - entity_manager_ptr_->setEntityStatusExternally(name, canonicalized); + setMapPose(name, entity_status.pose); + setTwist(name, entity_status.action_status.twist); + setAcceleration(name, entity_status.action_status.accel); } else { setEntityStatus(name, canonicalize(entity_status)); } diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 8cbb2cf0919..ed4b4e6c985 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace traffic_simulator { @@ -43,8 +44,8 @@ auto any(F f, T && x, Ts &&... xs) 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, double step_time) - -> std::optional + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, double step_time, + std::optional target_speed) -> std::optional { using math::arithmetic::isApproximatelyEqualTo; using math::arithmetic::isDefinitelyLessThan; @@ -92,11 +93,23 @@ auto makeUpdatedStatus( polyline_trajectory.shape.vertices.pop_back(); } - return makeUpdatedStatus(entity_status, polyline_trajectory, behavior_parameter, step_time); + return makeUpdatedStatus( + entity_status, polyline_trajectory, behavior_parameter, step_time, target_speed); }; auto is_infinity_or_nan = [](auto x) constexpr { return std::isinf(x) or std::isnan(x); }; + auto first_waypoint_with_arrival_time_specified = [&]() { + return std::find_if( + polyline_trajectory.shape.vertices.begin(), polyline_trajectory.shape.vertices.end(), + [](auto && vertex) { return not std::isnan(vertex.time); }); + }; + + auto is_breaking_waypoint = [&]() { + return first_waypoint_with_arrival_time_specified() >= + std::prev(polyline_trajectory.shape.vertices.end()); + }; + /* The following code implements the steering behavior known as "seek". See "Steering Behaviors For Autonomous Characters" by Craig Reynolds for more @@ -140,36 +153,33 @@ auto makeUpdatedStatus( distance_to_front_waypoint as the denominator if the distance miraculously becomes zero. */ - isApproximatelyEqualTo(distance_to_front_waypoint, 0.0)) { + isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse(); } else if ( const auto [distance, remaining_time] = [&]() { - if (const auto first_waypoint_with_arrival_time_specified = std::find_if( - std::begin(polyline_trajectory.shape.vertices), - std::end(polyline_trajectory.shape.vertices), - [](auto && vertex) { return not std::isnan(vertex.time); }); - first_waypoint_with_arrival_time_specified != - std::end(polyline_trajectory.shape.vertices)) { - /* - Note for anyone working on adding support for followingMode follow - to this function (FollowPolylineTrajectoryAction::tick) in the - future: if followingMode is follow, this distance calculation may be - inappropriate. - */ - auto total_distance_to = [&](auto last) { - auto total_distance = 0.0; - for (auto iter = std::begin(polyline_trajectory.shape.vertices); - 0 < std::distance(iter, last); ++iter) { - total_distance += hypot(iter->position.position, std::next(iter)->position.position); - } - return total_distance; - }; + /* + Note for anyone working on adding support for followingMode follow + to this function (FollowPolylineTrajectoryAction::tick) in the + future: if followingMode is follow, this distance calculation may be + inappropriate. + */ + auto total_distance_to = [&](auto last) { + auto total_distance = 0.0; + for (auto iter = std::begin(polyline_trajectory.shape.vertices); + 0 < std::distance(iter, last); ++iter) { + total_distance += hypot(iter->position.position, std::next(iter)->position.position); + } + return total_distance; + }; + if ( + first_waypoint_with_arrival_time_specified() != + std::end(polyline_trajectory.shape.vertices)) { if (const auto remaining_time = (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - first_waypoint_with_arrival_time_specified->time - entity_status.time; + first_waypoint_with_arrival_time_specified()->time - entity_status.time; /* The condition below should ideally be remaining_time < 0. @@ -197,22 +207,24 @@ auto makeUpdatedStatus( "Vehicle ", std::quoted(entity_status.name), " failed to reach the trajectory waypoint at the specified time. The specified time " "is ", - first_waypoint_with_arrival_time_specified->time, " (in ", + first_waypoint_with_arrival_time_specified()->time, " (in ", (not std::isnan(polyline_trajectory.base_time) ? "absolute" : "relative"), " simulation time). This may be due to unrealistic conditions of arrival time " "specification compared to vehicle parameters and dynamic constraints."); } else { return std::make_tuple( distance_to_front_waypoint + - total_distance_to(first_waypoint_with_arrival_time_specified), + total_distance_to(first_waypoint_with_arrival_time_specified()), remaining_time != 0 ? remaining_time : std::numeric_limits::epsilon()); } } else { return std::make_tuple( - distance_to_front_waypoint, std::numeric_limits::infinity()); + distance_to_front_waypoint + + total_distance_to(std::end(polyline_trajectory.shape.vertices) - 1), + std::numeric_limits::infinity()); } }(); - isApproximatelyEqualTo(distance, 0.0)) { + isDefinitelyLessThan(distance, std::numeric_limits::epsilon())) { return discard_the_front_waypoint_and_recurse(); } else if (const auto acceleration = entity_status.action_status.accel.linear.x; // [m/s^2] isinf(acceleration) or isnan(acceleration)) { @@ -250,44 +262,52 @@ auto makeUpdatedStatus( "following information to the developer: Vehicle ", std::quoted(entity_status.name), "'s speed value is NaN or infinity. The value is ", speed, "."); + } else if ( + /* + The controller provides the ability to calculate acceleration using constraints from the + behavior_parameter. The value is_breaking_waypoint() determines whether the calculated + acceleration takes braking into account - it is true if the nearest waypoint with the + specified time is the last waypoint or the nearest waypoint without the specified time is the + last waypoint. + + If an arrival time was specified for any of the remaining waypoints, priority is given to + meeting the arrival time, and the vehicle is driven at a speed at which the arrival time can + be met. + + However, the controller allows passing target_speed as a speed which is followed by the + controller. target_speed is passed only if no arrival time was specified for any of the + remaining waypoints. If despite no arrival time in the remaining waypoints, target_speed is + not set (it is std::nullopt), target_speed is assumed to be the same as max_speed from the + behaviour_parameter. + */ + const auto follow_waypoint_controller = FollowWaypointController( + behavior_parameter, step_time, is_breaking_waypoint(), + std::isinf(remaining_time) ? target_speed : std::nullopt); + false) { } else if ( /* The desired acceleration is the acceleration at which the destination can be reached exactly at the specified time (= time remaining at zero). - If no arrival time is specified for subsequent waypoints, there is no - need to accelerate or decelerate, so the current acceleration will be - the desired speed. + The desired acceleration is calculated to the nearest waypoint with a specified arrival time. + It is calculated in such a way as to reach a constant linear speed as quickly as possible, + ensuring arrival at a waypoint at the precise time and with the shortest possible distance. + More precisely, the controller selects acceleration to minimize the distance to the waypoint + that will be reached in a time step defined as the expected arrival time. + In addition, the controller ensures a smooth stop at the last waypoint of the trajectory, + with linear speed equal to zero and acceleration equal to zero. */ - const auto desired_acceleration = - [&]() { - if (std::isinf(remaining_time)) { - return acceleration; /// @todo Accelerate to match speed with `target_speed`. - } else { - /* - v [m/s] - ^ - | - desired_speed + /| - | / | - | / | - speed +/ | - | | - | | - +----+-------------> t [s] - 0 remaining_time - - desired_speed = speed + desired_acceleration * remaining_time - - distance = (speed + desired_speed) * remaining_time * 1/2 - - = (speed + speed + desired_acceleration * remaining_time) * remaining_time * 1/2 - - = speed * remaining_time + desired_acceleration * remaining_time^2 * 1/2 - */ - return 2 * distance / std::pow(remaining_time, 2) - 2 * speed / remaining_time; - } - }(); + const auto desired_acceleration = [&]() -> double { + try { + return follow_waypoint_controller.getAcceleration( + remaining_time, distance, acceleration, speed); + } catch (const ControllerError & e) { + throw common::Error( + "Vehicle ", std::quoted(entity_status.name), + " - controller operation problem encountered. ", + follow_waypoint_controller.getFollowedWaypointDetails(polyline_trajectory), e.what()); + } + }(); std::isinf(desired_acceleration) or std::isnan(desired_acceleration)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " @@ -295,15 +315,8 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), "'s desired acceleration value contains NaN or infinity. The value is ", desired_acceleration, "."); - } else if ( - /* - However, the desired acceleration is unrealistically large in terms of - vehicle performance and dynamic constraints, so it is clamped to a - realistic value. - */ - const auto desired_speed = - speed + std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time; - std::isinf(desired_speed) or std::isnan(desired_speed)) { + } else if (const auto desired_speed = speed + desired_acceleration * step_time; + std::isinf(desired_speed) or std::isnan(desired_speed)) { throw common::Error( "An error occurred in the internal state of FollowTrajectoryAction. Please report the " "following information to the developer: Vehicle ", @@ -338,13 +351,19 @@ auto makeUpdatedStatus( std::quoted(entity_status.name), "'s desired velocity contains NaN or infinity. The value is [", desired_velocity.x, ", ", desired_velocity.y, ", ", desired_velocity.z, "]."); + } else if (auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( + desired_acceleration, remaining_time_to_front_waypoint, distance, acceleration, + speed); + !std::isinf(remaining_time) && !predicted_state_opt.has_value()) { + throw common::Error( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: FollowWaypointController for vehicle ", + std::quoted(entity_status.name), + " calculated invalid acceleration:", " desired_acceleration: ", desired_acceleration, + ", remaining_time_to_front_waypoint: ", remaining_time_to_front_waypoint, + ", distance: ", distance, ", acceleration: ", acceleration, ", speed: ", speed, "."); } else { - /* - It's okay for this value to be infinite. - */ - const auto remaining_time_to_arrival_to_front_waypoint = - distance_to_front_waypoint / desired_speed; // [s] - + auto remaining_time_to_arrival_to_front_waypoint = predicted_state_opt->travel_time; if constexpr (false) { // clang-format off std::cout << std::fixed << std::boolalpha << std::string(80, '-') << std::endl; @@ -427,59 +446,52 @@ auto makeUpdatedStatus( // clang-format on } - /* - If the target point is reached during this step, it is considered - reached. - */ - if (isDefinitelyLessThan(remaining_time_to_arrival_to_front_waypoint, step_time)) { + if (std::isnan(remaining_time_to_front_waypoint)) { /* - The condition "Is remaining time to front waypoint less than remaining - time to arrival to front waypoint + step time?" means "If arrival is - next frame, is it too late?". This clause is executed only if the - front waypoint is expected to arrive during this frame. That is, the - conjunction of these conditions means "Did the vehicle arrive at the - front waypoint exactly on time?" Otherwise the vehicle will have - reached the front waypoint too early. - - This means that the vehicle did not slow down enough to reach the - current waypoint after passing the previous waypoint. In order to cope - with such situations, it is necessary to perform speed planning - considering not only the front of the waypoint queue but also the - waypoints after it. However, even with such speed planning, there is a - possibility that on-time arrival may not be possible depending on the - relationship between waypoint intervals, specified arrival times, - vehicle parameters, and dynamic restraints. For example, there is a - situation in which a large speed change is required over a short - distance while the permissible jerk is small. - - This implementation does simple velocity planning that considers only - the nearest waypoints in favor of simplicity of implementation. - - Note: There is no need to consider the case of arrival too late. - Because that case has already been verified when calculating the - remaining time. - - Note: If remaining time to front waypoint is nan, there is no need to - verify whether the arrival is too early. This arrival determination is - only interesting for the front waypoint. Verifying whether or not the - arrival time is specified in the front waypoint is exactly the - condition of "Is remaining time to front waypoint nan?" + If the nearest waypoint is arrived at in this step without a specific arrival time, it will + be considered as achieved */ - if ( - std::isnan(remaining_time_to_front_waypoint) or - isDefinitelyLessThan( - remaining_time_to_front_waypoint, - remaining_time_to_arrival_to_front_waypoint + step_time)) { + if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1) { + /* + If the trajectory has only waypoints with unspecified time, the last one is followed using + maximum speed including braking - in this case accuracy of arrival is checked + */ + if (follow_waypoint_controller.areConditionsOfArrivalMet( + acceleration, speed, distance_to_front_waypoint)) { + return discard_the_front_waypoint_and_recurse(); + } + } else { + /* + If it is an intermediate waypoint with an unspecified time, the accuracy of the arrival is + irrelevant + */ + if (auto this_step_distance = (speed + desired_acceleration * step_time) * step_time; + this_step_distance > distance_to_front_waypoint) { + return discard_the_front_waypoint_and_recurse(); + } + } + /* + If there is insufficient time left for the next calculation step. + The value of step_time/2 is compared, as the remaining time is affected by floating point + inaccuracy, sometimes it reaches values of 1e-7 (almost zero, but not zero) or (step_time - + 1e-7) (almost step_time). Because the step is fixed, it should be assumed that the value + here is either equal to 0 or step_time. Value step_time/2 allows to return true if no next + step is possible (remaining_time_to_front_waypoint is almost zero). + */ + } else if (isDefinitelyLessThan(remaining_time_to_front_waypoint, step_time / 2.0)) { + if (follow_waypoint_controller.areConditionsOfArrivalMet( + acceleration, speed, distance_to_front_waypoint)) { return discard_the_front_waypoint_and_recurse(); } else { throw common::SimulationError( - "Vehicle ", std::quoted(entity_status.name), " arrived at the waypoint in trajectory ", - remaining_time_to_front_waypoint, - " seconds earlier than the specified time. This may be due to unrealistic conditions of " - "arrival time specification compared to vehicle parameters and dynamic constraints."); + "Vehicle ", std::quoted(entity_status.name), " at time ", entity_status.time, + "s (remaining time is ", remaining_time_to_front_waypoint, + "s), has completed a trajectory to the nearest waypoint with", + " specified time equal to ", polyline_trajectory.shape.vertices.front().time, + "s at a distance equal to ", distance, + " from that waypoint which is greater than the accepted accuracy."); } } - const auto current_velocity = quaternion_operation::convertQuaternionToEulerAngle(entity_status.pose.orientation) * entity_status.action_status.twist.linear.x; @@ -498,15 +510,16 @@ auto makeUpdatedStatus( updated_status.pose.position += velocity * step_time; updated_status.pose.orientation = [&]() { - // do not change orientation if there is no velocity vector if (velocity.y == 0 && velocity.x == 0) { + // do not change orientation if there is no velocity vector return entity_status.pose.orientation; + } else { + geometry_msgs::msg::Vector3 direction; + direction.x = 0; + direction.y = 0; + direction.z = std::atan2(velocity.y, velocity.x); + return quaternion_operation::convertEulerAngleToQuaternion(direction); } - geometry_msgs::msg::Vector3 direction; - direction.x = 0; - direction.y = 0; - direction.z = std::atan2(velocity.y, velocity.x); - return quaternion_operation::convertEulerAngleToQuaternion(direction); }(); updated_status.action_status.twist.linear.x = norm(velocity); diff --git a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp new file mode 100644 index 00000000000..85b2a894fad --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp @@ -0,0 +1,446 @@ +// Copyright 2023 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 + +namespace traffic_simulator +{ +namespace follow_trajectory +{ +auto FollowWaypointController::getAnalyticalAccelerationForLastSteps( + const double remaining_time, const double remaining_distance, const double acceleration, + const double speed) const -> double +{ + if (remaining_time <= step_time * 2.0) { + if (!with_breaking || (std::abs(speed) < local_epsilon)) { + // Step in which the acceleration is set to 0.0. + return clampAcceleration(0.0, acceleration, speed); + } else { + // If it is the penultimate step with braking, speed should be equal to 0.0. + throw ControllerError( + "The speed in the penultimate step for last waypoint is different from zero, speed: ", + speed, ", acceleration: ", acceleration, ", remaining_time: ", remaining_time, + ", remaining_distance: ", remaining_distance, ". ", *this); + } + } else if (remaining_time <= step_time * 3.0) { + if (with_breaking) { + // Step in which the speed is set to 0.0. + return clampAcceleration(-speed / step_time, acceleration, speed); + } else { + /* + Step in which acceleration is set to ensure that the remaining + distance is traveled in next 2 steps. + */ + const double numerator = 2.0 / 3.0 * (remaining_distance - 2.0 * speed * step_time); + return clampAcceleration(numerator / std::pow(step_time, 2), acceleration, speed); + } + } else if (remaining_time <= step_time * 4.0) { + if (with_breaking) { + /* + Step in which acceleration is set to ensure that the remaining + distance is traveled in next step. + */ + const double numerator = 2.0 * (remaining_distance - speed * step_time); + return clampAcceleration(numerator / std::pow(step_time, 2), acceleration, speed); + } + } + + throw common::SemanticError( + "An analytical calculation of acceleration is not available for a time step greater than 4 (", + step_time * 4, "s) in the case of braking and greater than 3 (", step_time * 3, + "s) without braking."); +} + +auto FollowWaypointController::roundTimeToFullStepsWithTolerance( + const double remaining_time_source, const double time_tolerance) const -> double +{ + if (remaining_time_source >= std::numeric_limits::max() * step_time) { + throw ControllerError( + "Exceeded the range of the variable type (", remaining_time_source, "/", + step_time, ") - the value is too large. Probably the distance to the waypoint is extremely", + " large and the predicted time exceeds the range."); + } else { + const auto number_of_steps_source = static_cast(remaining_time_source / step_time); + const double remaining_time = number_of_steps_source * step_time; + if (const double time_difference = + (remaining_time_source / step_time) - (remaining_time / step_time); + time_difference > 1.0 - time_tolerance) { + return remaining_time + step_time + time_tolerance; + } else { + return remaining_time + time_tolerance; + } + } +} + +auto FollowWaypointController::getTimeRequiredForNonAcceleration(const double acceleration) const + -> double +{ + const double acceleration_rate = + (acceleration > 0.0) ? max_deceleration_rate : max_acceleration_rate; + return (std::abs(acceleration) / (acceleration_rate * step_time)) * step_time; +} + +auto FollowWaypointController::getAccelerationLimits( + const double acceleration, const double speed) const -> std::pair +{ + const auto time_for_non_acceleration = [&, acceleration]() { + if (std::abs(acceleration) < local_epsilon) { + return 0.0; + } else { + auto result = getTimeRequiredForNonAcceleration(acceleration); + return (result < step_time) ? step_time : result; + } + }(); + + const auto local_min_acceleration = [&]() { + const auto local_min_acceleration = acceleration - max_deceleration_rate * step_time; + if (std::abs(speed) < local_epsilon) { + // If the speed is equal to 0.0, it should no longer be decreased. + return std::max(0.0, local_min_acceleration); + } else if (time_for_non_acceleration > local_epsilon) { + // If the acceleration is not 0.0, ensure that there will be sufficient time to set it to 0.0. + return std::max(-speed / time_for_non_acceleration, local_min_acceleration); + } else { + /* + Otherwise, return an acceleration limited by constraints: it cannot be less than + -max_deceleration, it cannot be less than local_min_acceleration (resulting from the max + deceleration rate) and it cannot be less than -speed/step_time as this would result in a + negative speed. + */ + return std::max(-max_deceleration, std::max(local_min_acceleration, -speed / step_time)); + } + }(); + + const auto local_max_acceleration = [&]() { + const auto local_max_acceleration = acceleration + max_acceleration_rate * step_time; + if (std::abs(speed - target_speed) < local_epsilon) { + // If the speed is equal to target_speed, it should no longer be increased. + return std::min(0.0, local_max_acceleration); + } else if (speed > target_speed) { + // If speed is too high, assume that the max acceleration is equal to the min acceleration. + return local_min_acceleration; + } else if (time_for_non_acceleration > local_epsilon) { + // If the acceleration is not 0.0, ensure that there will be sufficient time to set it to 0.0. + return std::min((target_speed - speed) / time_for_non_acceleration, local_max_acceleration); + } else { + /* + Otherwise, return an acceleration limited by constraints: it cannot be greater than + max_acceleration, it cannot be greater than local_max_acceleration (resulting from the max + acceleration rate) and it cannot be greater than (target_speed-speed)/step_time as this + would result in a speed greater than target_speed. + */ + return std::min( + max_acceleration, std::min(local_max_acceleration, (target_speed - speed) / step_time)); + } + }(); + + /// @todo + if (local_max_acceleration < local_min_acceleration) { + // Such a case occurs, even without braking, it requires further investigation - this solves it. + return {local_max_acceleration, local_max_acceleration}; + } else { + // Check the validity of the limits. + const double speed_min = speed + local_min_acceleration * step_time; + const double speed_max = speed + local_max_acceleration * step_time; + if ( + speed_max < -local_epsilon || speed_max > max_speed || speed_min < -local_epsilon || + speed_min > max_speed) { + throw ControllerError( + "Incorrect acceleration limits [", local_min_acceleration, ", ", local_max_acceleration, + "] for acceleration: ", acceleration, " and speed: ", speed, " -> speed_min: ", speed_min, + " speed_max: ", speed_max, ". ", *this); + } else { + return {local_min_acceleration, local_max_acceleration}; + } + } +} + +auto FollowWaypointController::clampAcceleration( + const double candidate_acceleration, const double acceleration, const double speed) const + -> double +{ + auto [local_min_acceleration, local_max_acceleration] = + getAccelerationLimits(acceleration, speed); + return std::clamp(candidate_acceleration, local_min_acceleration, local_max_acceleration); +} + +auto FollowWaypointController::moveStraight( + PredictedState & state, const double candidate_acceleration) const -> void +{ + state.moveStraight( + clampAcceleration(candidate_acceleration, state.acceleration, state.speed), step_time); +} + +auto FollowWaypointController::getPredictedStopStateWithoutConsideringTime( + const double step_acceleration, const double remaining_distance, const double acceleration, + const double speed) const -> std::optional +{ + PredictedState breaking_check{acceleration, speed, 0.0, 0.0}; + moveStraight(breaking_check, step_acceleration); + while (!breaking_check.isImmobile(local_epsilon)) { + if (breaking_check.traveled_distance > remaining_distance + predicted_distance_tolerance) { + return std::nullopt; + } else { + moveStraight(breaking_check, breaking_check.acceleration - max_deceleration_rate * step_time); + } + } + return breaking_check; +} + +auto FollowWaypointController::getPredictedWaypointArrivalState( + const double step_acceleration, const double remaining_time, const double remaining_distance, + const double acceleration, const double speed) const -> std::optional +{ + const auto brakeUntilImmobility = [&](PredictedState & state) -> bool { + while (!state.isImmobile(local_epsilon)) { + if (state.travel_time >= remaining_time) { + return false; + } else { + moveStraight(state, state.acceleration - max_deceleration_rate * step_time); + } + } + return true; + }; + + PredictedState state{acceleration, speed, 0.0, 0.0}; + + if (remaining_time < step_time) { + return state; + } else { + // First step with acceleration equal to step_acceleration. + moveStraight(state, step_acceleration); + + if (with_breaking) { + // Predict the current (before acceleration zeroing) braking time required for stopping. + PredictedState breaking_check = state; + if (!brakeUntilImmobility(breaking_check)) { + // If complete immobility is not possible - ignore this candidate. + return std::nullopt; + } else if (std::abs(breaking_check.travel_time - remaining_time) <= step_time) { + // If it is breaking time - consider this candidate. + return breaking_check; + } + } + + // If it is not braking time, more time left for driving with constant speed. + while (std::abs(state.acceleration) > 0.0) { + if (state.travel_time >= remaining_time) { + throw ControllerError( + "It is not the braking time, but there is no time to achieve acceleration equal " + "to 0.0 - the trajectory does not meet the constraint of having an acceleration " + "equal to 0.0 on arrival at the followed waypoint, speed: ", + speed, ", acceleration: ", acceleration, ", remaining_time: ", remaining_time, + ", remaining_distance: ", remaining_distance, " step_acceleration: ", step_acceleration, + ". ", *this); + } else { + moveStraight(state, 0.0); + } + } + + if (std::abs(state.speed) <= local_epsilon) { + // If the previous steps caused the constant speed to be extremely low - ignore this. + return std::nullopt; + } else { + const double const_speed_value = state.speed; + + if (with_breaking) { + // Predict the current (after acceleration zeroing) braking time required for stopping. + if (!brakeUntilImmobility(state)) { + // If complete immobility is not possible - ignore this candidate. + return std::nullopt; + } else if (std::abs(state.travel_time - remaining_time) <= step_time) { + // If it is breaking time - consider this candidate. + return state; + } + } + + // Count the distance and time of movement with constant speed, use this to prediction. + if (const double const_speed_distance = remaining_distance - state.traveled_distance; + const_speed_distance >= std::numeric_limits::max() * const_speed_value) { + throw ControllerError( + "Exceeded the range of the variable type (", const_speed_distance, "/", + const_speed_value, ") - the value is too large. Probably the distance", + " to the waypoint is extremely large and the predicted time exceeds the range."); + } else { + const double const_speed_time = const_speed_distance / const_speed_value; + + // Round distance using a time equal to the full number of steps. + const double rounded_const_speed_time = + roundTimeToFullStepsWithTolerance(const_speed_time, step_time_tolerance); + const double rounded_const_speed_distance = rounded_const_speed_time * const_speed_value; + return PredictedState{ + state.acceleration, state.speed, rounded_const_speed_distance + state.traveled_distance, + rounded_const_speed_time + state.travel_time}; + } + } + } +} + +auto FollowWaypointController::getAcceleration( + const double remaining_distance, const double acceleration, const double speed) const -> double +{ + const auto [local_min_acceleration, local_max_acceleration] = + getAccelerationLimits(acceleration, speed); + + const double step_acceleration = + (local_max_acceleration - local_min_acceleration) / number_of_acceleration_candidates; + + auto min_distance_diff = std::numeric_limits::max(); + + std::optional best_acceleration = std::nullopt; + + for (std::size_t i = 0; i <= number_of_acceleration_candidates; ++i) { + const double candidate_acceleration = local_min_acceleration + i * step_acceleration; + + if (const auto predicted_state_opt = getPredictedStopStateWithoutConsideringTime( + candidate_acceleration, remaining_distance, acceleration, speed); + predicted_state_opt) { + if (const auto distance_diff = remaining_distance - predicted_state_opt->traveled_distance; + distance_diff >= 0 && + (std::abs(distance_diff) < std::abs(min_distance_diff) || min_distance_diff < 0)) { + min_distance_diff = distance_diff; + best_acceleration = candidate_acceleration; + } + } + } + + if (best_acceleration.has_value()) { + return best_acceleration.value(); + } else { + throw ControllerError( + "No acceleration found, speed: ", speed, ", acceleration: ", acceleration, + ", remaining_time: no specified, remaining_distance: ", remaining_distance, ". ", *this); + } +} + +auto FollowWaypointController::getAcceleration( + const double remaining_time_source, const double remaining_distance, const double acceleration, + const double speed) const -> double +{ + const auto [local_min_acceleration, local_max_acceleration] = + getAccelerationLimits(acceleration, speed); + + if ((speed + local_min_acceleration * step_time) * step_time > remaining_distance) { + /* + If in the current conditions, the waypoint will be reached in this step + even with the lowest possible acceleration set, this prevents cases in + which the controller is started extremely close to waypoint and nothing + can be done. + */ + return local_min_acceleration; + } else if (std::abs(local_min_acceleration - local_max_acceleration) < local_epsilon) { + /* + If the range is so tight that there is no choice. + */ + return local_min_acceleration; + } else if (std::isinf(remaining_time_source)) { + /* + If remaining time is no specified - this is the case when every next + point of the trajectory has no specified time. + */ + return getAcceleration(remaining_distance, acceleration, speed); + } else { + const auto remaining_time = + roundTimeToFullStepsWithTolerance(remaining_time_source, step_time_tolerance); + + /* + If last steps, increase accuracy by using analytical calculations. + */ + if (with_breaking && remaining_time <= step_time * 4) { + return getAnalyticalAccelerationForLastSteps( + remaining_time, remaining_distance, acceleration, speed); + } else if (remaining_time <= step_time * 3) { + return getAnalyticalAccelerationForLastSteps( + remaining_time, remaining_distance, acceleration, speed); + } + + // Create a lambda for candidate compare + auto min_time_diff = std::numeric_limits::max(); + auto min_distance_diff = std::numeric_limits::max(); + const auto isBetterCandidate = [local_epsilon = this->local_epsilon, &min_time_diff, + &min_distance_diff]( + double time_diff, double distance_diff) -> bool { + const bool same_time = std::abs(time_diff - min_time_diff) < local_epsilon; + const bool time_better = !same_time && (std::abs(time_diff) < std::abs(min_time_diff)); + const bool distance_better = + distance_diff >= 0 && + (std::abs(distance_diff) < std::abs(min_distance_diff) || min_distance_diff < 0); + return time_better || (same_time && distance_better); + }; + + // Create a lambda for candidate selection + std::optional best_acceleration = std::nullopt; + const auto considerCandidate = [&](double candidate_acceleration) -> void { + if ( + const auto predicted_state_opt = getPredictedWaypointArrivalState( + candidate_acceleration, remaining_time, remaining_distance, acceleration, speed)) { + const auto time_diff = remaining_time - predicted_state_opt->travel_time; + const auto distance_diff = remaining_distance - predicted_state_opt->traveled_distance; + if (isBetterCandidate(time_diff, distance_diff)) { + min_time_diff = time_diff; + min_distance_diff = distance_diff; + best_acceleration = candidate_acceleration; + } + } + }; + + // Consider the borderline values and precise value of 0 + considerCandidate(local_min_acceleration); + if (local_min_acceleration < 0.0 && local_max_acceleration > 0.0) { + considerCandidate(0.0); + } + considerCandidate(local_max_acceleration); + + /// @todo + /* + Find the best acceleration for followed waypoint with a specified time. + + It is likely that this search (in the intermediate values) can be + skipped if condition (min_time_diff < -step_time || min_time_diff > + step_time) is met however, at the moment this change affects other + behaviors - this requires further investigation. + */ + if (const double step_acceleration = + (local_max_acceleration - local_min_acceleration) / number_of_acceleration_candidates; + step_acceleration > local_epsilon) { + for (std::size_t i = 1; i < number_of_acceleration_candidates; ++i) { + considerCandidate(local_min_acceleration + i * step_acceleration); + } + } + + if (best_acceleration.has_value()) { + if (min_time_diff <= -step_time + step_time_tolerance) { + return local_max_acceleration; + } else if (min_time_diff >= step_time - step_time_tolerance) { + return local_min_acceleration; + } else if ( + std::abs(min_time_diff) < step_time && min_distance_diff > predicted_distance_tolerance) { + throw ControllerError( + "It is not possible to stop with the expected accuracy of the distance" + " from the waypoint - the trajectory does not meet the constraints. Check if the " + "specified " + "time is not too short for the constraints involved."); + } else { + return best_acceleration.value(); + } + } else { + throw ControllerError( + "No acceleration found, speed: ", speed, ", acceleration: ", acceleration, + ", remaining_time: ", remaining_time, ", remaining_time_source ", remaining_time_source, + ", remaining_distance: ", remaining_distance, ". ", *this); + } + } +} +} // namespace follow_trajectory +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/deleted_entity.cpp b/simulation/traffic_simulator/src/entity/deleted_entity.cpp deleted file mode 100644 index 9c2290cb108..00000000000 --- a/simulation/traffic_simulator/src/entity/deleted_entity.cpp +++ /dev/null @@ -1,69 +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 - -namespace traffic_simulator -{ -namespace entity -{ -DeletedEntity::DeletedEntity( - const std::string & name, const CanonicalizedEntityStatus & entity_status, - const std::shared_ptr & hdmap_utils_ptr) -: EntityBase(name, entity_status, hdmap_utils_ptr) -{ - 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.bounding_box.dimensions.x = 0; - status.bounding_box.dimensions.y = 0; - status.bounding_box.dimensions.z = 0; - status.pose.position.x = std::numeric_limits::infinity(); - status.pose.position.y = std::numeric_limits::infinity(); - status.pose.position.z = std::numeric_limits::infinity(); - status.lanelet_pose_valid = false; - status_ = CanonicalizedEntityStatus(status, hdmap_utils_ptr_); - status_before_update_ = CanonicalizedEntityStatus(status, hdmap_utils_ptr_); -} - -auto DeletedEntity::getCurrentAction() const -> std::string { return "static"; } - -auto DeletedEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter -{ - return traffic_simulator_msgs::msg::BehaviorParameter(); -} - -auto DeletedEntity::getDefaultDynamicConstraints() const - -> const traffic_simulator_msgs::msg::DynamicConstraints & -{ - static const auto default_dynamic_constraints = []() { - auto dynamic_constraints = traffic_simulator_msgs::msg::DynamicConstraints(); - dynamic_constraints.max_speed = 0.0; - dynamic_constraints.max_acceleration = 0.0; - dynamic_constraints.max_acceleration_rate = 0.0; - dynamic_constraints.max_deceleration = 0.0; - dynamic_constraints.max_deceleration_rate = 0.0; - return dynamic_constraints; - }(); - - return default_dynamic_constraints; -} - -auto DeletedEntity::fillLaneletPose(CanonicalizedEntityStatus &) -> void {} - -} // namespace entity -} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index b6f07378018..a9fb5ac3fc1 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -82,8 +82,7 @@ EgoEntity::EgoEntity( const traffic_simulator_msgs::msg::VehicleParameters & parameters, const Configuration & configuration) : VehicleEntity(name, entity_status, hdmap_utils_ptr, parameters), - field_operator_application(makeFieldOperatorApplication(configuration)), - externally_updated_status_(entity_status) + field_operator_application(makeFieldOperatorApplication(configuration)) { } @@ -155,7 +154,6 @@ auto EgoEntity::getWaypoints() -> const traffic_simulator_msgs::msg::WaypointsAr void EgoEntity::onUpdate(double current_time, double step_time) { EntityBase::onUpdate(current_time, step_time); - setStatus(externally_updated_status_); updateStandStillDuration(step_time); updateTraveledDistance(step_time); @@ -253,11 +251,6 @@ auto EgoEntity::setBehaviorParameter(const traffic_simulator_msgs::msg::Behavior { } -auto EgoEntity::setStatusExternally(const CanonicalizedEntityStatus & status) -> void -{ - externally_updated_status_ = status; -} - void EgoEntity::requestSpeedChange(double value, bool) { field_operator_application->restrictTargetSpeed(value); @@ -278,5 +271,45 @@ auto EgoEntity::fillLaneletPose(CanonicalizedEntityStatus & status) -> void EntityBase::fillLaneletPose(status, false); } +auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void +{ + const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets()); + std::optional lanelet_pose; + const auto get_matching_length = [&] { + return std::max( + vehicle_parameters.axles.front_axle.track_width, + vehicle_parameters.axles.rear_axle.track_width) * + 0.5 + + 1.0; + }; + if (unique_route_lanelets.empty()) { + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length()); + } else { + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(map_pose, unique_route_lanelets, get_matching_length()); + if (!lanelet_pose) { + lanelet_pose = + hdmap_utils_ptr_->toLaneletPose(map_pose, getBoundingBox(), false, get_matching_length()); + } + } + geometry_msgs::msg::Pose map_pose_z_fixed = map_pose; + auto status = static_cast(status_); + if (lanelet_pose) { + math::geometry::CatmullRomSpline spline( + hdmap_utils_ptr_->getCenterPoints(lanelet_pose->lanelet_id)); + if (const auto s_value = spline.getSValue(map_pose)) { + map_pose_z_fixed.position.z = spline.getPoint(s_value.value()).z; + } + status.pose = map_pose_z_fixed; + status.lanelet_pose_valid = true; + status.lanelet_pose = lanelet_pose.value(); + } else { + status.pose = map_pose; + status.lanelet_pose_valid = false; + status.lanelet_pose = LaneletPose(); + } + status_ = CanonicalizedEntityStatus(status, hdmap_utils_ptr_); +} } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index fff0eae220d..f29591fb621 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -738,6 +738,26 @@ void EntityBase::setTrafficLightManager( traffic_light_manager_ = traffic_light_manager; } +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, hdmap_utils_ptr_); +} + +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, hdmap_utils_ptr_); +} + +auto EntityBase::setMapPose(const geometry_msgs::msg::Pose &) -> void +{ + THROW_SEMANTIC_ERROR( + "You cannot set map pose to the vehicle other than ego named ", std::quoted(name), "."); +} + void EntityBase::activateOutOfRangeJob( double min_velocity, double max_velocity, double min_acceleration, double max_acceleration, double min_jerk, double max_jerk) diff --git a/simulation/traffic_simulator/src/entity/entity_manager.cpp b/simulation/traffic_simulator/src/entity/entity_manager.cpp index baf99026b2e..0fc5f2ecd73 100644 --- a/simulation/traffic_simulator/src/entity/entity_manager.cpp +++ b/simulation/traffic_simulator/src/entity/entity_manager.cpp @@ -86,13 +86,7 @@ visualization_msgs::msg::MarkerArray EntityManager::makeDebugMarker() const bool EntityManager::despawnEntity(const std::string & name) { - if (!entityExists(name)) { - return false; - } - - entities_[name].reset(new DeletedEntity(name, getEntityStatus(name), hdmap_utils_ptr_)); - - return true; + return entityExists(name) && entities_.erase(name); } bool EntityManager::entityExists(const std::string & name) @@ -141,11 +135,7 @@ auto EntityManager::getEntityNames() const -> const std::vector { std::vector names{}; for (const auto & each : entities_) { - // Add filter for DeletedEntity because this list is used on SimpleSensorSimulator which do not - // know DeletedEntity. - if (each.second->getEntityType().type != DeletedEntity::ENTITY_TYPE_ID) { - names.push_back(each.first); - } + names.push_back(each.first); } return names; } @@ -721,18 +711,6 @@ auto EntityManager::setEntityStatus( } } -auto EntityManager::setEntityStatusExternally( - const std::string & name, const CanonicalizedEntityStatus & status) -> void -{ - if (not isEgo(name)) { - THROW_SEMANTIC_ERROR( - "You cannot set entity status externally to the vehicle other than ego named ", - std::quoted(name), "."); - } else { - dynamic_cast(entities_[name].get())->setStatusExternally(status); - } -} - void EntityManager::setVerbose(const bool verbose) { configuration.verbose = verbose; diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index 675f97af028..743726912fb 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -31,6 +31,7 @@ PedestrianEntity::PedestrianEntity( const std::string & plugin_name) : EntityBase(name, entity_status, hdmap_utils_ptr), plugin_name(plugin_name), + pedestrian_parameters(parameters), loader_(pluginlib::ClassLoader( "traffic_simulator", "entity_behavior::BehaviorPluginBase")), behavior_plugin_ptr_(loader_.createSharedInstance(plugin_name)), diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index f2718d6ff2f..f07e8355d23 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -31,6 +31,7 @@ VehicleEntity::VehicleEntity( const traffic_simulator_msgs::msg::VehicleParameters & parameters, const std::string & plugin_name) : EntityBase(name, entity_status, hdmap_utils_ptr), + vehicle_parameters(parameters), loader_(pluginlib::ClassLoader( "traffic_simulator", "entity_behavior::BehaviorPluginBase")), behavior_plugin_ptr_(loader_.createSharedInstance(plugin_name)), @@ -135,7 +136,6 @@ void VehicleEntity::onUpdate(double current_time, double step_time) behavior_plugin_ptr_->setEntityTypeList(entity_type_list_); behavior_plugin_ptr_->setEntityStatus(std::make_unique(status_)); behavior_plugin_ptr_->setTargetSpeed(target_speed_); - auto route_lanelets = getRouteLanelets(); behavior_plugin_ptr_->setRouteLanelets(route_lanelets); diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 94ab54ce7fd..c17cebc71c6 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -218,7 +218,7 @@ auto HdMapUtils::getLaneletIds() const -> lanelet::Ids return ids; } -auto HdMapUtils::getLaneletPolygon(lanelet::Id lanelet_id) const +auto HdMapUtils::getLaneletPolygon(const lanelet::Id lanelet_id) const -> std::vector { std::vector points; @@ -251,8 +251,8 @@ auto HdMapUtils::filterLaneletIds(const lanelet::Ids & lanelet_ids, const char s } auto HdMapUtils::getNearbyLaneletIds( - const geometry_msgs::msg::Point & position, double distance_threshold, - std::size_t search_count) const -> lanelet::Ids + const geometry_msgs::msg::Point & position, const double distance_threshold, + const std::size_t search_count) const -> lanelet::Ids { lanelet::Ids lanelet_ids; lanelet::BasicPoint2d search_point(position.x, position.y); @@ -270,8 +270,8 @@ auto HdMapUtils::getNearbyLaneletIds( } auto HdMapUtils::getNearbyLaneletIds( - const geometry_msgs::msg::Point & point, double distance_thresh, bool include_crosswalk, - std::size_t search_count) const -> lanelet::Ids + const geometry_msgs::msg::Point & point, const double distance_thresh, + const bool include_crosswalk, const std::size_t search_count) const -> lanelet::Ids { lanelet::Ids lanelet_ids; lanelet::BasicPoint2d search_point(point.x, point.y); @@ -310,7 +310,8 @@ auto HdMapUtils::getHeight(const traffic_simulator_msgs::msg::LaneletPose & lane } auto HdMapUtils::getCollisionPointInLaneCoordinate( - lanelet::Id lanelet_id, lanelet::Id crossing_lanelet_id) const -> std::optional + const lanelet::Id lanelet_id, const lanelet::Id crossing_lanelet_id) const + -> std::optional { namespace bg = boost::geometry; using Point = bg::model::d2::point_xy; @@ -392,8 +393,8 @@ auto HdMapUtils::getConflictingCrosswalkIds(const lanelet::Ids & lanelet_ids) co } auto HdMapUtils::clipTrajectoryFromLaneletIds( - lanelet::Id lanelet_id, double s, const lanelet::Ids & lanelet_ids, double forward_distance) const - -> std::vector + const lanelet::Id lanelet_id, const double s, const lanelet::Ids & lanelet_ids, + const double forward_distance) const -> std::vector { std::vector ret; bool on_traj = false; @@ -491,7 +492,7 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan auto HdMapUtils::matchToLane( const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - bool include_crosswalk, double reduction_ratio) const -> std::optional + const bool include_crosswalk, const double reduction_ratio) const -> std::optional { std::optional id; lanelet::matching::Object2d obj; @@ -537,8 +538,8 @@ auto HdMapUtils::matchToLane( } auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, bool include_crosswalk, double matching_distance) const - -> std::optional + const geometry_msgs::msg::Pose & pose, const bool include_crosswalk, + const double matching_distance) const -> std::optional { const auto lanelet_ids = getNearbyLaneletIds(pose.position, 0.1, include_crosswalk); if (lanelet_ids.empty()) { @@ -554,8 +555,8 @@ auto HdMapUtils::toLaneletPose( } auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, lanelet::Id lanelet_id, double matching_distance) const - -> std::optional + const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id, + const double matching_distance) const -> std::optional { const auto spline = getCenterPointsSpline(lanelet_id); const auto s = spline->getSValue(pose, matching_distance); @@ -588,7 +589,7 @@ auto HdMapUtils::toLaneletPose( auto HdMapUtils::toLaneletPose( const geometry_msgs::msg::Pose & pose, const lanelet::Ids & lanelet_ids, - double matching_distance) const -> std::optional + const double matching_distance) const -> std::optional { for (const auto id : lanelet_ids) { if (const auto lanelet_pose = toLaneletPose(pose, id, matching_distance); lanelet_pose) { @@ -600,7 +601,7 @@ auto HdMapUtils::toLaneletPose( auto HdMapUtils::toLaneletPose( const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - bool include_crosswalk, double matching_distance) const + const bool include_crosswalk, const double matching_distance) const -> std::optional { const auto lanelet_id = matchToLane(pose, bbox, include_crosswalk); @@ -629,8 +630,9 @@ auto HdMapUtils::toLaneletPose( } auto HdMapUtils::toLaneletPoses( - const geometry_msgs::msg::Pose & pose, lanelet::Id lanelet_id, double matching_distance, - bool include_opposite_direction) const -> std::vector + const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id, + const double matching_distance, const bool include_opposite_direction) const + -> std::vector { std::vector ret; traffic_simulator_msgs::msg::EntityType type; @@ -649,8 +651,8 @@ auto HdMapUtils::toLaneletPoses( } auto HdMapUtils::getClosestLaneletId( - const geometry_msgs::msg::Pose & pose, double distance_thresh, bool include_crosswalk) const - -> std::optional + const geometry_msgs::msg::Pose & pose, const double distance_thresh, + const bool include_crosswalk) const -> std::optional { lanelet::BasicPoint2d search_point(pose.position.x, pose.position.y); std::vector> nearest_lanelet = @@ -695,8 +697,8 @@ auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double } auto HdMapUtils::getLaneChangeableLaneletId( - lanelet::Id lanelet_id, traffic_simulator::lane_change::Direction direction, - std::uint8_t shift) const -> std::optional + const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction, + const std::uint8_t shift) const -> std::optional { if (shift == 0) { return getLaneChangeableLaneletId( @@ -719,7 +721,7 @@ auto HdMapUtils::getLaneChangeableLaneletId( } auto HdMapUtils::getLaneChangeableLaneletId( - lanelet::Id lanelet_id, traffic_simulator::lane_change::Direction direction) const + const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction) const -> std::optional { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -742,7 +744,8 @@ auto HdMapUtils::getLaneChangeableLaneletId( return target; } -auto HdMapUtils::getPreviousLanelets(lanelet::Id lanelet_id, double distance) const -> lanelet::Ids +auto HdMapUtils::getPreviousLanelets(const lanelet::Id lanelet_id, const double distance) const + -> lanelet::Ids { lanelet::Ids ret; double total_distance = 0.0; @@ -750,16 +753,14 @@ auto HdMapUtils::getPreviousLanelets(lanelet::Id lanelet_id, double distance) co while (total_distance < distance) { auto ids = getPreviousLaneletIds(lanelet_id, "straight"); if (ids.size() != 0) { - lanelet_id = ids[0]; - total_distance = total_distance + getLaneletLength(lanelet_id); - ret.push_back(lanelet_id); + total_distance = total_distance + getLaneletLength(ids[0]); + ret.push_back(ids[0]); continue; } else { auto else_ids = getPreviousLaneletIds(lanelet_id); if (else_ids.size() != 0) { - lanelet_id = else_ids[0]; - total_distance = total_distance + getLaneletLength(lanelet_id); - ret.push_back(lanelet_id); + total_distance = total_distance + getLaneletLength(else_ids[0]); + ret.push_back(else_ids[0]); continue; } else { break; @@ -769,7 +770,7 @@ auto HdMapUtils::getPreviousLanelets(lanelet::Id lanelet_id, double distance) co return ret; } -auto HdMapUtils::isInRoute(lanelet::Id lanelet_id, const lanelet::Ids & route) const -> bool +auto HdMapUtils::isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & route) const -> bool { return std::find_if(route.begin(), route.end(), [lanelet_id](const auto id) { return lanelet_id == id; @@ -777,8 +778,8 @@ auto HdMapUtils::isInRoute(lanelet::Id lanelet_id, const lanelet::Ids & route) c } auto HdMapUtils::getFollowingLanelets( - lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, double distance, - bool include_self) const -> lanelet::Ids + const lanelet::Id lanelet_id, const lanelet::Ids & candidate_lanelet_ids, const double distance, + const bool include_self) const -> lanelet::Ids { if (candidate_lanelet_ids.empty()) { return {}; @@ -815,24 +816,26 @@ auto HdMapUtils::getFollowingLanelets( } auto HdMapUtils::getFollowingLanelets( - lanelet::Id lanelet_id, double distance, bool include_self) const -> lanelet::Ids + const lanelet::Id lanelet_id, const double distance, const bool include_self) const + -> lanelet::Ids { lanelet::Ids ret; double total_distance = 0.0; if (include_self) { ret.push_back(lanelet_id); } + lanelet::Id end_lanelet_id = lanelet_id; while (total_distance < distance) { - if (const auto straight_ids = getNextLaneletIds(lanelet_id, "straight"); - straight_ids.size() != 0) { - lanelet_id = straight_ids[0]; - total_distance = total_distance + getLaneletLength(lanelet_id); - ret.push_back(lanelet_id); + if (const auto straight_ids = getNextLaneletIds(end_lanelet_id, "straight"); + !straight_ids.empty()) { + total_distance = total_distance + getLaneletLength(straight_ids[0]); + ret.push_back(straight_ids[0]); + end_lanelet_id = straight_ids[0]; continue; - } else if (const auto ids = getNextLaneletIds(lanelet_id); ids.size() != 0) { - lanelet_id = ids[0]; - total_distance = total_distance + getLaneletLength(lanelet_id); - ret.push_back(lanelet_id); + } else if (const auto ids = getNextLaneletIds(end_lanelet_id); ids.size() != 0) { + total_distance = total_distance + getLaneletLength(ids[0]); + ret.push_back(ids[0]); + end_lanelet_id = ids[0]; continue; } else { break; @@ -841,7 +844,7 @@ auto HdMapUtils::getFollowingLanelets( return ret; } -auto HdMapUtils::getRoute(lanelet::Id from_lanelet_id, lanelet::Id to_lanelet_id) const +auto HdMapUtils::getRoute(const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id) const -> lanelet::Ids { if (route_cache_.exists(from_lanelet_id, to_lanelet_id)) { @@ -868,7 +871,7 @@ auto HdMapUtils::getRoute(lanelet::Id from_lanelet_id, lanelet::Id to_lanelet_id return ids; } -auto HdMapUtils::getCenterPointsSpline(lanelet::Id lanelet_id) const +auto HdMapUtils::getCenterPointsSpline(const lanelet::Id lanelet_id) const -> std::shared_ptr { getCenterPoints(lanelet_id); @@ -889,7 +892,7 @@ auto HdMapUtils::getCenterPoints(const lanelet::Ids & lanelet_ids) const return ret; } -auto HdMapUtils::getCenterPoints(lanelet::Id lanelet_id) const +auto HdMapUtils::getCenterPoints(const lanelet::Id lanelet_id) const -> std::vector { std::vector ret; @@ -928,7 +931,7 @@ auto HdMapUtils::getCenterPoints(lanelet::Id lanelet_id) const return ret; } -auto HdMapUtils::getLaneletLength(lanelet::Id lanelet_id) const -> double +auto HdMapUtils::getLaneletLength(const lanelet::Id lanelet_id) const -> double { if (lanelet_length_cache_.exists(lanelet_id)) { return lanelet_length_cache_.getLength(lanelet_id); @@ -938,7 +941,7 @@ auto HdMapUtils::getLaneletLength(lanelet::Id lanelet_id) const -> double return ret; } -auto HdMapUtils::getPreviousRoadShoulderLanelet(lanelet::Id lanelet_id) const -> lanelet::Ids +auto HdMapUtils::getPreviousRoadShoulderLanelet(const lanelet::Id lanelet_id) const -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -950,7 +953,7 @@ auto HdMapUtils::getPreviousRoadShoulderLanelet(lanelet::Id lanelet_id) const -> return ids; } -auto HdMapUtils::getPreviousLaneletIds(lanelet::Id lanelet_id) const -> lanelet::Ids +auto HdMapUtils::getPreviousLaneletIds(const lanelet::Id lanelet_id) const -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -958,7 +961,7 @@ auto HdMapUtils::getPreviousLaneletIds(lanelet::Id lanelet_id) const -> lanelet: ids.push_back(llt.id()); } for (const auto & id : getPreviousRoadShoulderLanelet(lanelet_id)) { - ids.emplace_back(id); + ids.push_back(id); } return ids; } @@ -973,7 +976,7 @@ auto HdMapUtils::getPreviousLaneletIds(const lanelet::Ids & lanelet_ids) const - } auto HdMapUtils::getPreviousLaneletIds( - lanelet::Id lanelet_id, const std::string & turn_direction) const -> lanelet::Ids + const lanelet::Id lanelet_id, const std::string & turn_direction) const -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -995,7 +998,7 @@ auto HdMapUtils::getPreviousLaneletIds( return sortAndUnique(ids); } -auto HdMapUtils::getNextRoadShoulderLanelet(lanelet::Id lanelet_id) const -> lanelet::Ids +auto HdMapUtils::getNextRoadShoulderLanelet(const lanelet::Id lanelet_id) const -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -1007,7 +1010,7 @@ auto HdMapUtils::getNextRoadShoulderLanelet(lanelet::Id lanelet_id) const -> lan return ids; } -auto HdMapUtils::getNextLaneletIds(lanelet::Id lanelet_id) const -> lanelet::Ids +auto HdMapUtils::getNextLaneletIds(const lanelet::Id lanelet_id) const -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -1029,8 +1032,8 @@ auto HdMapUtils::getNextLaneletIds(const lanelet::Ids & lanelet_ids) const -> la return sortAndUnique(ids); } -auto HdMapUtils::getNextLaneletIds(lanelet::Id lanelet_id, const std::string & turn_direction) const - -> lanelet::Ids +auto HdMapUtils::getNextLaneletIds( + const lanelet::Id lanelet_id, const std::string & turn_direction) const -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); @@ -1072,7 +1075,7 @@ auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids } auto HdMapUtils::getTrafficLightBulbPosition( - lanelet::Id traffic_light_id, const std::string & color_name) const + const lanelet::Id traffic_light_id, const std::string & color_name) const -> std::optional { lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); @@ -1108,7 +1111,7 @@ auto HdMapUtils::getTrafficLightBulbPosition( } auto HdMapUtils::getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from_pose, double along) const + const traffic_simulator_msgs::msg::LaneletPose & from_pose, const double along) const -> traffic_simulator_msgs::msg::LaneletPose { traffic_simulator_msgs::msg::LaneletPose along_pose = from_pose; @@ -1145,21 +1148,21 @@ auto HdMapUtils::getAlongLaneletPose( return along_pose; } -auto HdMapUtils::getLeftBound(lanelet::Id lanelet_id) const +auto HdMapUtils::getLeftBound(const lanelet::Id lanelet_id) const -> std::vector { return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).leftBound()); } -auto HdMapUtils::getRightBound(lanelet::Id lanelet_id) const +auto HdMapUtils::getRightBound(const lanelet::Id lanelet_id) const -> std::vector { return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).rightBound()); } auto HdMapUtils::getLeftLaneletIds( - lanelet::Id lanelet_id, traffic_simulator_msgs::msg::EntityType type, - bool include_opposite_direction) const -> lanelet::Ids + const lanelet::Id lanelet_id, const traffic_simulator_msgs::msg::EntityType & type, + const bool include_opposite_direction) const -> lanelet::Ids { switch (type.type) { case traffic_simulator_msgs::msg::EntityType::EGO: @@ -1284,8 +1287,8 @@ auto HdMapUtils::getLaneChangeTrajectory( auto HdMapUtils::getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from_pose, const traffic_simulator::lane_change::Parameter & lane_change_parameter, - double maximum_curvature_threshold, double target_trajectory_length, - double forward_distance_threshold) const + const double maximum_curvature_threshold, const double target_trajectory_length, + const double forward_distance_threshold) const -> std::optional> { double to_length = getLaneletLength(lane_change_parameter.target.lanelet_id); @@ -1328,11 +1331,12 @@ auto HdMapUtils::getLaneChangeTrajectory( const geometry_msgs::msg::Pose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, const traffic_simulator::lane_change::TrajectoryShape trajectory_shape, - double tangent_vector_size) const -> math::geometry::HermiteCurve + const double tangent_vector_size) const -> math::geometry::HermiteCurve { geometry_msgs::msg::Vector3 start_vec; geometry_msgs::msg::Vector3 to_vec; geometry_msgs::msg::Pose goal_pose = toMapPose(to_pose).pose; + double tangent_vector_size_in_curve = 0.0; switch (trajectory_shape) { case traffic_simulator::lane_change::TrajectoryShape::CUBIC: start_vec = getVectorFromPose(from_pose, tangent_vector_size); @@ -1343,25 +1347,26 @@ auto HdMapUtils::getLaneChangeTrajectory( "Failed to calculate tangent vector at lanelet_id : ", to_pose.lanelet_id, " s : ", to_pose.s); } + tangent_vector_size_in_curve = tangent_vector_size; break; case traffic_simulator::lane_change::TrajectoryShape::LINEAR: start_vec.x = (goal_pose.position.x - from_pose.position.x); start_vec.y = (goal_pose.position.y - from_pose.position.y); start_vec.z = (goal_pose.position.z - from_pose.position.z); to_vec = start_vec; - tangent_vector_size = 1; + tangent_vector_size_in_curve = 1; break; } - geometry_msgs::msg::Vector3 goal_vec = to_vec; - goal_vec.x = goal_vec.x * tangent_vector_size; - goal_vec.y = goal_vec.y * tangent_vector_size; - goal_vec.z = goal_vec.z * tangent_vector_size; - math::geometry::HermiteCurve curve(from_pose, goal_pose, start_vec, goal_vec); - return curve; + return math::geometry::HermiteCurve( + from_pose, goal_pose, start_vec, + geometry_msgs::build() + .x(to_vec.x * tangent_vector_size_in_curve) + .y(to_vec.y * tangent_vector_size_in_curve) + .z(to_vec.z * tangent_vector_size_in_curve)); } -auto HdMapUtils::getVectorFromPose(const geometry_msgs::msg::Pose & pose, double magnitude) const - -> geometry_msgs::msg::Vector3 +auto HdMapUtils::getVectorFromPose(const geometry_msgs::msg::Pose & pose, const double magnitude) + const -> geometry_msgs::msg::Vector3 { geometry_msgs::msg::Vector3 dir = quaternion_operation::convertQuaternionToEulerAngle(pose.orientation); @@ -1372,12 +1377,12 @@ auto HdMapUtils::getVectorFromPose(const geometry_msgs::msg::Pose & pose, double return vector; } -auto HdMapUtils::isInLanelet(lanelet::Id lanelet_id, double s) const -> bool +auto HdMapUtils::isInLanelet(const lanelet::Id lanelet_id, const double s) const -> bool { return 0 <= s and s <= getCenterPointsSpline(lanelet_id)->getLength(); } -auto HdMapUtils::toMapPoints(lanelet::Id lanelet_id, const std::vector & s) const +auto HdMapUtils::toMapPoints(const lanelet::Id lanelet_id, const std::vector & s) const -> std::vector { std::vector ret; @@ -1417,13 +1422,14 @@ auto HdMapUtils::toMapPose(const traffic_simulator_msgs::msg::LaneletPose & lane } } -auto HdMapUtils::getTangentVector(lanelet::Id lanelet_id, double s) const +auto HdMapUtils::getTangentVector(const lanelet::Id lanelet_id, const double s) const -> std::optional { return getCenterPointsSpline(lanelet_id)->getTangentVector(s); } -auto HdMapUtils::canChangeLane(lanelet::Id from_lanelet_id, lanelet::Id to_lanelet_id) const -> bool +auto HdMapUtils::canChangeLane( + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id) const -> bool { const auto from_lanelet = lanelet_map_ptr_->laneletLayer.get(from_lanelet_id); const auto to_lanelet = lanelet_map_ptr_->laneletLayer.get(to_lanelet_id); @@ -1600,7 +1606,7 @@ auto HdMapUtils::getRightOfWayLaneletIds(const lanelet::Ids & lanelet_ids) const return ret; } -auto HdMapUtils::getRightOfWayLaneletIds(lanelet::Id lanelet_id) const -> lanelet::Ids +auto HdMapUtils::getRightOfWayLaneletIds(const lanelet::Id lanelet_id) const -> lanelet::Ids { lanelet::Ids ids; for (const auto & right_of_way : @@ -1614,7 +1620,7 @@ auto HdMapUtils::getRightOfWayLaneletIds(lanelet::Id lanelet_id) const -> lanele return ids; } -auto HdMapUtils::getTrafficSignRegElementsOnPath(const lanelet::Ids & lanelet_ids) const +auto HdMapUtils::getTrafficSignRegulatoryElementsOnPath(const lanelet::Ids & lanelet_ids) const -> std::vector> { std::vector> ret; @@ -1622,13 +1628,27 @@ auto HdMapUtils::getTrafficSignRegElementsOnPath(const lanelet::Ids & lanelet_id const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); const auto traffic_signs = lanelet.regulatoryElementsAs(); for (const auto & traffic_sign : traffic_signs) { - ret.push_back(traffic_sign); + ret.emplace_back(traffic_sign); } } return ret; } -auto HdMapUtils::getTrafficLightRegElementsOnPath(const lanelet::Ids & lanelet_ids) const +auto HdMapUtils::getTrafficSignRegulatoryElements() const + -> std::vector> +{ + std::vector> ret; + for (const auto & lanelet_id : getLaneletIds()) { + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); + const auto traffic_signs = lanelet.regulatoryElementsAs(); + for (const auto & traffic_sign : traffic_signs) { + ret.emplace_back(traffic_sign); + } + } + return ret; +} + +auto HdMapUtils::getTrafficLightRegulatoryElementsOnPath(const lanelet::Ids & lanelet_ids) const -> std::vector> { std::vector> ret; @@ -1637,7 +1657,20 @@ auto HdMapUtils::getTrafficLightRegElementsOnPath(const lanelet::Ids & lanelet_i const auto traffic_lights = lanelet.regulatoryElementsAs(); for (const auto & traffic_light : traffic_lights) { - ret.push_back(traffic_light); + ret.emplace_back(traffic_light); + } + } + return ret; +} + +auto HdMapUtils::getStopLines() const -> lanelet::ConstLineStrings3d +{ + lanelet::ConstLineStrings3d ret; + for (const auto & traffic_sign : getTrafficSignRegulatoryElements()) { + if (traffic_sign->type() == "stop_sign") { + for (const auto & stop_line : traffic_sign->refLines()) { + ret.emplace_back(stop_line); + } } } return ret; @@ -1647,18 +1680,25 @@ auto HdMapUtils::getStopLinesOnPath(const lanelet::Ids & lanelet_ids) const -> lanelet::ConstLineStrings3d { lanelet::ConstLineStrings3d ret; - const auto traffic_signs = getTrafficSignRegElementsOnPath(lanelet_ids); - for (const auto & traffic_sign : traffic_signs) { - if (traffic_sign->type() != "stop_sign") { - continue; - } - for (const auto & stop_line : traffic_sign->refLines()) { - ret.push_back(stop_line); + for (const auto & traffic_sign : getTrafficSignRegulatoryElementsOnPath(lanelet_ids)) { + if (traffic_sign->type() == "stop_sign") { + for (const auto & stop_line : traffic_sign->refLines()) { + ret.emplace_back(stop_line); + } } } return ret; } +auto HdMapUtils::getStopLineIds() const -> lanelet::Ids +{ + lanelet::Ids stop_line_ids; + for (const auto & ret : getStopLines()) { + stop_line_ids.push_back(ret.id()); + } + return stop_line_ids; +} + auto HdMapUtils::getStopLineIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids { lanelet::Ids stop_line_ids; @@ -1702,7 +1742,7 @@ auto HdMapUtils::getTrafficLightStopLineIds(const lanelet::Id traffic_light_id) return ids; } -auto HdMapUtils::getTrafficLightStopLinesPoints(lanelet::Id traffic_light_id) const +auto HdMapUtils::getTrafficLightStopLinesPoints(const lanelet::Id traffic_light_id) const -> std::vector> { std::vector> ret; @@ -1724,7 +1764,7 @@ auto HdMapUtils::getTrafficLightStopLinesPoints(lanelet::Id traffic_light_id) co return ret; } -auto HdMapUtils::getStopLinePolygon(lanelet::Id lanelet_id) const +auto HdMapUtils::getStopLinePolygon(const lanelet::Id lanelet_id) const -> std::vector { std::vector points; @@ -1742,7 +1782,7 @@ auto HdMapUtils::getStopLinePolygon(lanelet::Id lanelet_id) const auto HdMapUtils::getTrafficLightIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids { lanelet::Ids ids; - for (const auto & traffic_light : getTrafficLightRegElementsOnPath(route_lanelets)) { + for (const auto & traffic_light : getTrafficLightRegulatoryElementsOnPath(route_lanelets)) { for (auto light_string : traffic_light->lightBulbs()) { if (light_string.hasAttribute("traffic_light_id")) { if (auto id = light_string.attribute("traffic_light_id").asId(); id) { diff --git a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp index df996c0dff3..30f1c5081bf 100644 --- a/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp +++ b/simulation/traffic_simulator/src/simulation_clock/simulation_clock.cpp @@ -17,16 +17,19 @@ namespace traffic_simulator { -SimulationClock::SimulationClock(double realtime_factor, double frame_rate) +SimulationClock::SimulationClock(bool use_sim_time, double realtime_factor, double frame_rate) : rclcpp::Clock(RCL_ROS_TIME), - use_raw_clock(true), + use_sim_time(use_sim_time), realtime_factor(realtime_factor), - frame_rate(frame_rate), - time_on_initialize(now()) + frame_rate_(frame_rate), + time_at_the_start_of_the_simulator_(use_sim_time ? 0 : now().nanoseconds()) { } -auto SimulationClock::update() -> void { ++frame_; } +auto SimulationClock::update() -> void +{ + seconds_since_the_simulator_started_ += realtime_factor / frame_rate_; +} auto SimulationClock::getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock { @@ -37,10 +40,11 @@ auto SimulationClock::getCurrentRosTimeAsMsg() -> rosgraph_msgs::msg::Clock auto SimulationClock::getCurrentRosTime() -> rclcpp::Time { - if (use_raw_clock) { + if (not use_sim_time) { return now(); } else { - return time_on_initialize + rclcpp::Duration::from_seconds(getCurrentSimulationTime()); + return time_at_the_start_of_the_simulator_ + + rclcpp::Duration::from_seconds(getCurrentSimulationTime()); } } @@ -50,7 +54,7 @@ auto SimulationClock::start() -> void THROW_SIMULATION_ERROR( "npc logic is already started. Please check simulation clock instance was destroyed."); } else { - frame_offset_ = frame_; + seconds_at_the_start_of_the_scenario_ = seconds_since_the_simulator_started_; } } } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp index 27e5e669e4f..fe983240de8 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_manager.cpp @@ -49,6 +49,15 @@ auto TrafficLightManager::getTrafficLight(const lanelet::Id traffic_light_id) -> } } +auto TrafficLightManager::getTrafficLightIds() const -> const lanelet::Ids +{ + lanelet::Ids traffic_light_ids; + for (const auto & traffic_light_ : getTrafficLights()) { + traffic_light_ids.push_back(traffic_light_.first); + } + return traffic_light_ids; +} + auto TrafficLightManager::getTrafficLights() const -> const TrafficLightMap & { return traffic_lights_; diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 3d25341b13d..9190bfc22a4 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package openscenario_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Revert "feat: add deleted entity to traffic simulator" + This reverts commit ba2abf393757a53e266476fc7f4184cf495837af. +* Revert "feat: remove DELETED entity type by using internal id" + This reverts commit a15268f290e4957fbcfce1e3c52c37de23852a4c. +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Kotaro Yoshimoto, Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* feat: remove DELETED entity type by using internal id +* feat: add deleted entity to traffic simulator +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Michał Kiełczykowski, Piotr Zyskowski, f0reachARR, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into feature/perception_ground_truth diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index b7f066cb922..63526be2916 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 0.8.0 + 1.0.0 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 1df743dd0a2..d6aa1ed0f49 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -2,6 +2,100 @@ Changelog for package random_test_runner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into revert/1096 +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* fix error in linelint +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* Merge pull request `#1162 `_ from tier4/fix/linelint + fix linelint error +* fix linelint error +* Merge pull request `#1143 `_ from RobotecAI/random-test-runner-docs-update + Update of Random Test Runner documentation +* random_test_runner executable name changed +* Merge branch 'tier4:master' into random-test-runner-docs-update +* chore: fix linelint errors +* Merge pull request `#1152 `_ from RobotecAI/feature/test-random-test-runner + Feature/test random test runner +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* TestExecutor tests +* YamlTestParamsSaver tests +* Add remaining JunitXmlReporter tests +* Add some JunitXmlReporter tests +* Add TestRandomizer tests +* Move helper function +* Fix and add randomizer tests +* Add Randomizers tests +* Add lanelet map and LaneletUtils tests +* Add random_test_runner tests +* Updated random test runner docs +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Contributors: Kotaro Yoshimoto, Masaya Kataoka, Mateusz Palczuk, Paweł Lech, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Merge pull request `#1128 `_ from RobotecAI/fix/random-test-runner-fix +* rvalue reference used in executeWithErrorHandling +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* Merge branch 'master' into fix/duplicated_nodes +* Exceptions handling in TestExecutor +* Separate traffic_simulator::API for each TestExecutor +* OccupancyGridSensor attachment and allow_goal_modification parameter declaration +* initialize_duration added to launch file +* Merge pull request `#1111 `_ from tier4/feature/traffic_light_confidence +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* chore: suppress warning from boost library +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into fix/rtc_command_action/continuous_execution +* Merge pull request `#1098 `_ from tier4/fix/port_document +* change default port to 5555 +* change default port to 8080 +* change port +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into refactor/lanelet-id +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge pull request `#1087 `_ from tier4/feature/drop_galactic_support +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/drop_galactic_support +* Merge pull request `#1027 `_ from tier4/feature/new_traffic_light +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* remove workbound for galactic +* chore: change architecture_type to awf/universe/20230906 +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* refactor: update architecture_type format +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* feat: add new architecture_type awf/universe/2023.08 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into ref/RJD-553_restore_repeated_update_entity_status diff --git a/test_runner/random_test_runner/CMakeLists.txt b/test_runner/random_test_runner/CMakeLists.txt index d503ad51bd1..349a15df94d 100644 --- a/test_runner/random_test_runner/CMakeLists.txt +++ b/test_runner/random_test_runner/CMakeLists.txt @@ -37,16 +37,14 @@ include(FindProtobuf REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_executable(random_test_runner +ament_auto_add_library(${PROJECT_NAME} SHARED src/data_types.cpp src/test_randomizer.cpp - src/test_executor.cpp src/lanelet_utils.cpp src/random_test_runner.cpp - src/random_test_runner_node.cpp ) -target_link_libraries(random_test_runner +target_link_libraries(${PROJECT_NAME} ${PROTOBUF_LIBRARY} ${YAML_CPP_LIBRARIES} pthread @@ -55,14 +53,19 @@ target_link_libraries(random_test_runner fmt ) +ament_auto_add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}_node.cpp +) + install( - DIRECTORY launch rviz include param + DIRECTORY launch rviz include param test/map DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + add_subdirectory(test) endif() ament_auto_package() diff --git a/test_runner/random_test_runner/include/random_test_runner/random_test_runner.hpp b/test_runner/random_test_runner/include/random_test_runner/random_test_runner.hpp index b69bf50aee1..74b27ed9546 100644 --- a/test_runner/random_test_runner/include/random_test_runner/random_test_runner.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/random_test_runner.hpp @@ -50,8 +50,8 @@ class RandomTestRunner : public rclcpp::Node std::random_device seed_randomization_device_; - std::vector test_executors_; - std::vector::iterator current_test_executor_; + std::vector> test_executors_; + std::vector>::iterator current_test_executor_; JunitXmlReporter error_reporter_; 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 0f6be36379c..2255a105304 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 @@ -27,23 +27,220 @@ #include "random_test_runner/metrics/goal_reached_metric.hpp" #include "traffic_simulator/api/api.hpp" +const double test_timeout = 60.0; + +traffic_simulator_msgs::msg::VehicleParameters getVehicleParameters() +{ + traffic_simulator_msgs::msg::VehicleParameters parameters; + parameters.name = "vehicle.volkswagen.t"; + parameters.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + parameters.performance.max_speed = 69.444; + parameters.performance.max_acceleration = 200; + parameters.performance.max_deceleration = 10.0; + parameters.bounding_box.center.x = 1.5; + parameters.bounding_box.center.y = 0.0; + parameters.bounding_box.center.z = 0.9; + parameters.bounding_box.dimensions.x = 4.5; + parameters.bounding_box.dimensions.y = 2.1; + parameters.bounding_box.dimensions.z = 1.8; + parameters.axles.front_axle.max_steering = 0.5; + parameters.axles.front_axle.wheel_diameter = 0.6; + parameters.axles.front_axle.track_width = 1.8; + parameters.axles.front_axle.position_x = 3.1; + parameters.axles.front_axle.position_z = 0.3; + parameters.axles.rear_axle.max_steering = 0.0; + parameters.axles.rear_axle.wheel_diameter = 0.6; + parameters.axles.rear_axle.track_width = 1.8; + parameters.axles.rear_axle.position_x = 0.0; + parameters.axles.rear_axle.position_z = 0.3; + return parameters; +} + +template class TestExecutor { public: TestExecutor( - std::shared_ptr api, TestDescription description, + std::shared_ptr api, TestDescription description, JunitXmlReporterTestCase test_case_reporter, SimulatorType simulator_type, - ArchitectureType architecture_type, rclcpp::Logger logger); + ArchitectureType architecture_type, rclcpp::Logger logger) + : api_(std::move(api)), + test_description_(std::move(description)), + error_reporter_(std::move(test_case_reporter)), + simulator_type_(simulator_type), + architecture_type_(architecture_type), + logger_(logger) + { + } + + void initialize() + { + executeWithErrorHandling([this]() { + std::string message = fmt::format("Test description: {}", test_description_); + RCLCPP_INFO_STREAM(logger_, message); + scenario_completed_ = false; + + api_->updateFrame(); + + if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { + api_->spawn( + ego_name_, api_->canonicalize(test_description_.ego_start_position), + getVehicleParameters(), traffic_simulator::VehicleBehavior::autoware()); + api_->setEntityStatus( + ego_name_, api_->canonicalize(test_description_.ego_start_position), + traffic_simulator::helper::constructActionStatus()); + + if (architecture_type_ == ArchitectureType::AWF_UNIVERSE) { + api_->attachLidarSensor(traffic_simulator::helper::constructLidarConfiguration( + traffic_simulator::helper::LidarType::VLP16, ego_name_, + stringFromArchitectureType(architecture_type_))); + + double constexpr detection_update_duration = 0.1; + api_->attachDetectionSensor( + traffic_simulator::helper::constructDetectionSensorConfiguration( + ego_name_, stringFromArchitectureType(architecture_type_), + detection_update_duration)); + + api_->attachOccupancyGridSensor([&]() { + simulation_api_schema::OccupancyGridSensorConfiguration configuration; + configuration.set_architecture_type(stringFromArchitectureType(architecture_type_)); + configuration.set_entity(ego_name_); + configuration.set_filter_by_range(true); + configuration.set_height(200); + configuration.set_range(300); + configuration.set_resolution(0.5); + configuration.set_update_duration(0.1); + configuration.set_width(200); + return configuration; + }()); + + api_->asFieldOperatorApplication(ego_name_).template declare_parameter( + "allow_goal_modification", true); + } + + // XXX dirty hack: wait for autoware system to launch + // ugly but helps for now + std::this_thread::sleep_for(std::chrono::milliseconds{5000}); + + api_->requestAssignRoute( + ego_name_, std::vector({api_->canonicalize(test_description_.ego_goal_position)})); + api_->asFieldOperatorApplication(ego_name_).engage(); + + goal_reached_metric_.setGoal(test_description_.ego_goal_pose); + } + + for (size_t i = 0; i < test_description_.npcs_descriptions.size(); i++) { + const auto & npc_descr = test_description_.npcs_descriptions[i]; + api_->spawn( + npc_descr.name, api_->canonicalize(npc_descr.start_position), getVehicleParameters()); + api_->setEntityStatus( + npc_descr.name, api_->canonicalize(npc_descr.start_position), + traffic_simulator::helper::constructActionStatus(npc_descr.speed)); + api_->requestSpeedChange(npc_descr.name, npc_descr.speed, true); + } + }); + } + + void update() + { + executeWithErrorHandling([this]() { + if (!api_->isEgoSpawned() && !api_->isNpcLogicStarted()) { + api_->startNpcLogic(); + } + if ( + api_->isEgoSpawned() && !api_->isNpcLogicStarted() && + api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) { + api_->startNpcLogic(); + } + + auto current_time = api_->getCurrentTime(); + + if (!std::isnan(current_time)) { + bool timeout_reached = current_time >= test_timeout; + if (timeout_reached) { + if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { + if (!goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) { + RCLCPP_INFO(logger_, "Timeout reached"); + error_reporter_.reportTimeout(); + } + } + scenario_completed_ = true; + + return; + } + } + + if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { + for (const auto & npc : test_description_.npcs_descriptions) { + if (api_->entityExists(npc.name) && api_->checkCollision(ego_name_, npc.name)) { + if (ego_collision_metric_.isThereEgosCollisionWith(npc.name, current_time)) { + std::string message = + fmt::format("New collision occurred between ego and {}", npc.name); + RCLCPP_INFO_STREAM(logger_, message); + error_reporter_.reportCollision(npc, current_time); + } + } + } + + if (almost_standstill_metric_.isAlmostStandingStill(api_->getEntityStatus(ego_name_))) { + RCLCPP_INFO(logger_, "Standstill duration exceeded"); + if (goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) { + RCLCPP_INFO(logger_, "Goal reached, standstill expected"); + } else { + error_reporter_.reportStandStill(); + } + scenario_completed_ = true; + } + } + + api_->updateFrame(); + }); + } + + void deinitialize() + { + executeWithErrorHandling([this]() { + std::string message = fmt::format("Deinitialize: {}", test_description_); + RCLCPP_INFO_STREAM(logger_, message); + + if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { + api_->despawn(ego_name_); + } + for (const auto & npc : test_description_.npcs_descriptions) { + api_->despawn(npc.name); + } + }); + } - void initialize(); - void update(); - void deinitialize(); - bool scenarioCompleted(); + bool scenarioCompleted() { return scenario_completed_; } private: - void executeWithErrorHandling(std::function && func); + void executeWithErrorHandling(std::function && func) + { + try { + func(); + } catch (const common::AutowareError & error) { + error_reporter_.reportException("autoware error", error.what()); + std::string message = fmt::format("common::AutowareError occurred: {}", error.what()); + RCLCPP_ERROR_STREAM(logger_, message); + scenario_completed_ = true; + } catch (const common::scenario_simulator_exception::Error & error) { + error_reporter_.reportException("scenario simulator error", error.what()); + std::string message = + fmt::format("common::scenario_simulator_exception::Error occurred: {}", error.what()); + RCLCPP_ERROR_STREAM(logger_, message); + scenario_completed_ = true; + } catch (const std::runtime_error & error) { + std::string message = fmt::format("std::runtime_error occurred: {}", error.what()); + RCLCPP_ERROR_STREAM(logger_, message); + scenario_completed_ = true; + } catch (...) { + RCLCPP_ERROR(logger_, "Unknown error occurred."); + scenario_completed_ = true; + } + } - std::shared_ptr api_; + std::shared_ptr api_; TestDescription test_description_; const std::string ego_name_ = "ego"; diff --git a/test_runner/random_test_runner/launch/random_test.launch.py b/test_runner/random_test_runner/launch/random_test.launch.py index d9123fca05f..7d67f58c712 100644 --- a/test_runner/random_test_runner/launch/random_test.launch.py +++ b/test_runner/random_test_runner/launch/random_test.launch.py @@ -34,7 +34,7 @@ def architecture_types(): - return ["awf/auto", "awf/universe", "awf/universe/20230906", "tier4/proposal"] + return ["awf/universe"] def default_autoware_launch_package_of(architecture_type): @@ -44,10 +44,7 @@ def default_autoware_launch_package_of(architecture_type): ) return { - "awf/auto": "scenario_simulator_launch", - "awf/universe": "autoware_launch", - "awf/universe/20230906": "autoware_launch", - "tier4/proposal": "autoware_launch", + "awf/universe": "autoware_launch" }[architecture_type] @@ -58,10 +55,7 @@ def default_autoware_launch_file_of(architecture_type): ) return { - "awf/auto": "autoware_auto.launch.py", - "awf/universe": "planning_simulator.launch.xml", - "awf/universe/20230906": "planning_simulator.launch.xml", - "tier4/proposal": "planning_simulator.launch.xml", + "awf/universe": "planning_simulator.launch.xml" }[architecture_type] @@ -72,9 +66,9 @@ def __init__(self): self.autoware_launch_arguments = { # autoware arguments # - "architecture_type": {"default": "awf/auto", "description": "Autoware architecture type", "values": architecture_types()}, - "sensor_model": {"default": "aip_xx1", "description": "Ego sensor model"}, - "vehicle_model": {"default": "lexus", "description": "Ego vehicle model"}, + "architecture_type": {"default": "awf/universe", "description": "Autoware architecture type", "values": architecture_types()}, + "sensor_model": {"default": "sample_sensor_kit", "description": "Ego sensor model"}, + "vehicle_model": {"default": "sample_vehicle", "description": "Ego vehicle model"}, } self.random_test_arguments = { @@ -83,7 +77,7 @@ def __init__(self): "description": "Yaml filename within random_test_runner/param directory containing test parameters." "If specified (not empty), other test arguments will be ignored"}, "simulator_type": {"default": "simple_sensor_simulator", "description": "Simulation backend", - "values": ["simple_sensor_simulator", "unity"]}, + "values": ["simple_sensor_simulator"]}, "simulator_host": {"default": "localhost", "description": "Simulation host. It can be either IP address " @@ -208,9 +202,9 @@ def launch_setup(self, context, *args, **kwargs): scenario_node = Node( package="random_test_runner", - executable="random_test_runner", + executable="random_test_runner_node", namespace="simulation", - name="random_test_runner", + name="random_test_runner_node", output="screen", arguments=[("__log_level:=info")], parameters=parameters diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 48a3ef208ce..61900fc9ed2 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 0.8.0 + 1.0.0 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 @@ -20,6 +20,8 @@ ament_lint_auto ament_cmake_clang_format ament_cmake_copyright + ament_cmake_gtest + ament_cmake_gmock ament_cmake_lint_cmake ament_cmake_pep257 ament_cmake_xmllint diff --git a/test_runner/random_test_runner/src/test_executor.cpp b/test_runner/random_test_runner/src/test_executor.cpp deleted file mode 100644 index c2001a37b8c..00000000000 --- a/test_runner/random_test_runner/src/test_executor.cpp +++ /dev/null @@ -1,228 +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. -// -// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. - -#include "random_test_runner/test_executor.hpp" - -#include - -#include "random_test_runner/file_interactions/junit_xml_reporter.hpp" -#include "random_test_runner/file_interactions/yaml_test_params_saver.hpp" - -const double test_timeout = 60.0; - -traffic_simulator_msgs::msg::VehicleParameters getVehicleParameters() -{ - traffic_simulator_msgs::msg::VehicleParameters parameters; - parameters.name = "vehicle.volkswagen.t"; - parameters.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; - parameters.performance.max_speed = 69.444; - parameters.performance.max_acceleration = 200; - parameters.performance.max_deceleration = 10.0; - parameters.bounding_box.center.x = 1.5; - parameters.bounding_box.center.y = 0.0; - parameters.bounding_box.center.z = 0.9; - parameters.bounding_box.dimensions.x = 4.5; - parameters.bounding_box.dimensions.y = 2.1; - parameters.bounding_box.dimensions.z = 1.8; - parameters.axles.front_axle.max_steering = 0.5; - parameters.axles.front_axle.wheel_diameter = 0.6; - parameters.axles.front_axle.track_width = 1.8; - parameters.axles.front_axle.position_x = 3.1; - parameters.axles.front_axle.position_z = 0.3; - parameters.axles.rear_axle.max_steering = 0.0; - parameters.axles.rear_axle.wheel_diameter = 0.6; - parameters.axles.rear_axle.track_width = 1.8; - parameters.axles.rear_axle.position_x = 0.0; - parameters.axles.rear_axle.position_z = 0.3; - return parameters; -} - -TestExecutor::TestExecutor( - std::shared_ptr api, TestDescription description, - JunitXmlReporterTestCase test_case_reporter, SimulatorType simulator_type, - ArchitectureType architecture_type, rclcpp::Logger logger) -: api_(std::move(api)), - test_description_(std::move(description)), - error_reporter_(std::move(test_case_reporter)), - simulator_type_(simulator_type), - architecture_type_(architecture_type), - logger_(logger) -{ -} - -void TestExecutor::initialize() -{ - executeWithErrorHandling([this]() { - std::string message = fmt::format("Test description: {}", test_description_); - RCLCPP_INFO_STREAM(logger_, message); - scenario_completed_ = false; - - api_->updateFrame(); - - if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { - api_->spawn( - ego_name_, api_->canonicalize(test_description_.ego_start_position), getVehicleParameters(), - traffic_simulator::VehicleBehavior::autoware()); - api_->setEntityStatus( - ego_name_, api_->canonicalize(test_description_.ego_start_position), - traffic_simulator::helper::constructActionStatus()); - - if (architecture_type_ == ArchitectureType::AWF_UNIVERSE) { - api_->attachLidarSensor(traffic_simulator::helper::constructLidarConfiguration( - traffic_simulator::helper::LidarType::VLP16, ego_name_, - stringFromArchitectureType(architecture_type_))); - - double constexpr detection_update_duration = 0.1; - api_->attachDetectionSensor( - traffic_simulator::helper::constructDetectionSensorConfiguration( - ego_name_, stringFromArchitectureType(architecture_type_), detection_update_duration)); - - api_->attachOccupancyGridSensor([&]() { - simulation_api_schema::OccupancyGridSensorConfiguration configuration; - configuration.set_architecture_type(stringFromArchitectureType(architecture_type_)); - configuration.set_entity(ego_name_); - configuration.set_filter_by_range(true); - configuration.set_height(200); - configuration.set_range(300); - configuration.set_resolution(0.5); - configuration.set_update_duration(0.1); - configuration.set_width(200); - return configuration; - }()); - - api_->asFieldOperatorApplication(ego_name_).declare_parameter( - "allow_goal_modification", true); - } - - // XXX dirty hack: wait for autoware system to launch - // ugly but helps for now - std::this_thread::sleep_for(std::chrono::milliseconds{5000}); - - api_->requestAssignRoute( - ego_name_, std::vector( - {api_->canonicalize(test_description_.ego_goal_position)})); - api_->asFieldOperatorApplication(ego_name_).engage(); - - goal_reached_metric_.setGoal(test_description_.ego_goal_pose); - } - - for (size_t i = 0; i < test_description_.npcs_descriptions.size(); i++) { - const auto & npc_descr = test_description_.npcs_descriptions[i]; - api_->spawn( - npc_descr.name, api_->canonicalize(npc_descr.start_position), getVehicleParameters()); - api_->setEntityStatus( - npc_descr.name, api_->canonicalize(npc_descr.start_position), - traffic_simulator::helper::constructActionStatus(npc_descr.speed)); - api_->requestSpeedChange(npc_descr.name, npc_descr.speed, true); - } - }); -} - -void TestExecutor::update() -{ - executeWithErrorHandling([this]() { - if (!api_->isEgoSpawned() && !api_->isNpcLogicStarted()) { - api_->startNpcLogic(); - } - if ( - api_->isEgoSpawned() && !api_->isNpcLogicStarted() && - api_->asFieldOperatorApplication(api_->getEgoName()).engageable()) { - api_->startNpcLogic(); - } - - auto current_time = api_->getCurrentTime(); - - if (!std::isnan(current_time)) { - bool timeout_reached = current_time >= test_timeout; - if (timeout_reached) { - if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { - if (!goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) { - RCLCPP_INFO(logger_, "Timeout reached"); - error_reporter_.reportTimeout(); - } - } - scenario_completed_ = true; - return; - } - } - if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { - for (const auto & npc : test_description_.npcs_descriptions) { - if (api_->entityExists(npc.name) && api_->checkCollision(ego_name_, npc.name)) { - if (ego_collision_metric_.isThereEgosCollisionWith(npc.name, current_time)) { - std::string message = - fmt::format("New collision occurred between ego and {}", npc.name); - RCLCPP_INFO_STREAM(logger_, message); - error_reporter_.reportCollision(npc, current_time); - } - } - } - - if (almost_standstill_metric_.isAlmostStandingStill(api_->getEntityStatus(ego_name_))) { - RCLCPP_INFO(logger_, "Standstill duration exceeded"); - if (goal_reached_metric_.isGoalReached(api_->getEntityStatus(ego_name_))) { - RCLCPP_INFO(logger_, "Goal reached, standstill expected"); - } else { - error_reporter_.reportStandStill(); - } - scenario_completed_ = true; - } - } - - api_->updateFrame(); - }); -} - -void TestExecutor::deinitialize() -{ - executeWithErrorHandling([this]() { - std::string message = fmt::format("Deinitialize: {}", test_description_); - RCLCPP_INFO_STREAM(logger_, message); - - if (simulator_type_ == SimulatorType::SIMPLE_SENSOR_SIMULATOR) { - api_->despawn(ego_name_); - } - for (const auto & npc : test_description_.npcs_descriptions) { - api_->despawn(npc.name); - } - }); -} - -void TestExecutor::executeWithErrorHandling(std::function && func) -{ - try { - func(); - } catch (const common::AutowareError & error) { - error_reporter_.reportException("autoware error", error.what()); - std::string message = fmt::format("common::AutowareError occurred: {}", error.what()); - RCLCPP_ERROR_STREAM(logger_, message); - scenario_completed_ = true; - } catch (const common::scenario_simulator_exception::Error & error) { - error_reporter_.reportException("scenario simulator error", error.what()); - std::string message = - fmt::format("common::scenario_simulator_exception::Error occurred: {}", error.what()); - RCLCPP_ERROR_STREAM(logger_, message); - scenario_completed_ = true; - } catch (const std::runtime_error & error) { - std::string message = fmt::format("std::runtime_error occurred: {}", error.what()); - RCLCPP_ERROR_STREAM(logger_, message); - scenario_completed_ = true; - } catch (...) { - RCLCPP_ERROR(logger_, "Unknown error occurred."); - scenario_completed_ = true; - } -} - -bool TestExecutor::scenarioCompleted() { return scenario_completed_; } diff --git a/test_runner/random_test_runner/test/CMakeLists.txt b/test_runner/random_test_runner/test/CMakeLists.txt new file mode 100644 index 00000000000..7e8f497dd36 --- /dev/null +++ b/test_runner/random_test_runner/test/CMakeLists.txt @@ -0,0 +1,38 @@ +# 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. +# +# Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +ament_add_gtest(test_almost_standstill_metric test_almost_standstill_metric.cpp) +ament_add_gtest(test_ego_collision_metric test_ego_collision_metric.cpp) +ament_add_gtest(test_goal_reached_metric test_goal_reached_metric.cpp) +ament_add_gtest(test_data_types test_data_types.cpp) +ament_add_gtest(test_lanelet_utils test_lanelet_utils.cpp) +ament_add_gtest(test_randomizers test_randomizers.cpp) +ament_add_gtest(test_test_randomizer test_test_randomizer.cpp) +ament_add_gtest(test_junit_xml_reporter test_junit_xml_reporter.cpp) +ament_add_gtest(test_yaml_test_params_saver test_yaml_test_params_saver.cpp) +ament_add_gmock(test_test_executor test_test_executor.cpp) + + +target_link_libraries(test_almost_standstill_metric ${PROJECT_NAME}) +target_link_libraries(test_ego_collision_metric ${PROJECT_NAME}) +target_link_libraries(test_goal_reached_metric ${PROJECT_NAME}) +target_link_libraries(test_data_types ${PROJECT_NAME}) +target_link_libraries(test_lanelet_utils ${PROJECT_NAME}) +target_link_libraries(test_randomizers ${PROJECT_NAME}) +target_link_libraries(test_test_randomizer ${PROJECT_NAME}) +target_link_libraries(test_junit_xml_reporter ${PROJECT_NAME}) +target_link_libraries(test_yaml_test_params_saver ${PROJECT_NAME}) +target_link_libraries(test_test_executor ${PROJECT_NAME}) diff --git a/test_runner/random_test_runner/test/expect_eq_macros.hpp b/test_runner/random_test_runner/test/expect_eq_macros.hpp new file mode 100644 index 00000000000..b5995b0bab0 --- /dev/null +++ b/test_runner/random_test_runner/test/expect_eq_macros.hpp @@ -0,0 +1,63 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#ifndef RANDOM_TEST_RUNNER__TEST__EXPECT_EQ_MACROS_HPP_ +#define RANDOM_TEST_RUNNER__TEST__EXPECT_EQ_MACROS_HPP_ + +#include + +#include + +#define EXPECT_LANELET_POSE_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_EQ(DATA0.lanelet_id, DATA1.lanelet_id); \ + EXPECT_NEAR(DATA0.s, DATA1.s, TOLERANCE); + +#define EXPECT_POINT_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ + EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); + +#define EXPECT_POINT_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); + +#define EXPECT_QUATERNION_EQ(DATA0, DATA1) \ + EXPECT_DOUBLE_EQ(DATA0.x, DATA1.x); \ + EXPECT_DOUBLE_EQ(DATA0.y, DATA1.y); \ + EXPECT_DOUBLE_EQ(DATA0.z, DATA1.z); \ + EXPECT_DOUBLE_EQ(DATA0.w, DATA1.w); + +#define EXPECT_QUATERNION_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_NEAR(DATA0.x, DATA1.x, TOLERANCE); \ + EXPECT_NEAR(DATA0.y, DATA1.y, TOLERANCE); \ + EXPECT_NEAR(DATA0.z, DATA1.z, TOLERANCE); \ + EXPECT_NEAR(DATA0.w, DATA1.w, TOLERANCE); + +#define EXPECT_POSE_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_POINT_NEAR(DATA0.position, DATA1.position, TOLERANCE); \ + EXPECT_QUATERNION_NEAR(DATA0.orientation, DATA1.orientation, TOLERANCE); + +#define EXPECT_IN_RANGE(VAL, MIN, MAX) \ + EXPECT_GE((VAL), (MIN)); \ + EXPECT_LE((VAL), (MAX)) + +#define EXPECT_NPC_DESCRIPTION_NEAR(DATA0, DATA1, TOLERANCE) \ + EXPECT_STREQ(DATA0.name.c_str(), DATA1.name.c_str()); \ + EXPECT_NEAR(DATA0.speed, DATA1.speed, TOLERANCE); \ + EXPECT_LANELET_POSE_NEAR(DATA0.start_position, DATA1.start_position, TOLERANCE); + +#endif // RANDOM_TEST_RUNNER__TEST__EXPECT_EQ_MACROS_HPP_ diff --git a/test_runner/random_test_runner/test/map/lanelet2_map.osm b/test_runner/random_test_runner/test/map/lanelet2_map.osm new file mode 100644 index 00000000000..077647823b7 --- /dev/null +++ b/test_runner/random_test_runner/test/map/lanelet2_map.osm @@ -0,0 +1,5346 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test_runner/random_test_runner/test/map/with_road_shoulder/lanelet2_map.osm b/test_runner/random_test_runner/test/map/with_road_shoulder/lanelet2_map.osm new file mode 100644 index 00000000000..d7e7eed74fa --- /dev/null +++ b/test_runner/random_test_runner/test/map/with_road_shoulder/lanelet2_map.osm @@ -0,0 +1,6259 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test_runner/random_test_runner/test/test_almost_standstill_metric.cpp b/test_runner/random_test_runner/test/test_almost_standstill_metric.cpp new file mode 100644 index 00000000000..5905eac7363 --- /dev/null +++ b/test_runner/random_test_runner/test/test_almost_standstill_metric.cpp @@ -0,0 +1,64 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +#include "test_utils.hpp" + +TEST(Metrics, AlmostStandstillMetric_move) +{ + AlmostStandstillMetric metric; + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(0.0, 1.0, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(2.0, 1.0, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(4.0, 1.0, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(6.0, 1.0, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(8.0, 1.0, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(10.0, 1.0, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(12.0, 1.0, 0.0))); +} + +TEST(Metrics, AlmostStandstillMetric_almostStandstill) +{ + AlmostStandstillMetric metric; + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(0.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(2.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(4.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(6.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(8.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(10.0, 1e-3, 0.0))); + EXPECT_TRUE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(10.001, 1e-3, 0.0))); + EXPECT_TRUE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(12.0, 1e-3, 0.0))); +} + +TEST(Metrics, AlmostStandstillMetric_dataTimeout) +{ + AlmostStandstillMetric metric; + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(0.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(2.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(4.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(8.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(10.0, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(10.001, 1e-3, 0.0))); + EXPECT_FALSE(metric.isAlmostStandingStill(getCanonicalizedEntityStatus(12.0, 1e-3, 09.0))); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_data_types.cpp b/test_runner/random_test_runner/test/test_data_types.cpp new file mode 100644 index 00000000000..d271ed262e6 --- /dev/null +++ b/test_runner/random_test_runner/test/test_data_types.cpp @@ -0,0 +1,78 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +#include "test_utils.hpp" + +TEST(DataTypes, simulatorTypeFromString_correct) +{ + EXPECT_EQ( + simulatorTypeFromString("simple_sensor_simulator"), SimulatorType::SIMPLE_SENSOR_SIMULATOR); + EXPECT_EQ(simulatorTypeFromString("unity"), SimulatorType::UNITY); +} + +TEST(DataTypes, simulatorTypeFromString_incorrect) +{ + EXPECT_THROW(simulatorTypeFromString("unknown"), std::runtime_error); + EXPECT_THROW(simulatorTypeFromString("simple_sensor_simulator "), std::runtime_error); + EXPECT_THROW(simulatorTypeFromString(" simple_sensor_simulator"), std::runtime_error); + EXPECT_THROW(simulatorTypeFromString("unity "), std::runtime_error); + EXPECT_THROW(simulatorTypeFromString(" unity"), std::runtime_error); +} + +TEST(DataTypes, architectureTypeFromString_correct) +{ + EXPECT_EQ(architectureTypeFromString("awf/auto"), ArchitectureType::AWF_AUTO); + EXPECT_EQ(architectureTypeFromString("awf/universe"), ArchitectureType::AWF_UNIVERSE); + EXPECT_EQ(architectureTypeFromString("-awf/universe"), ArchitectureType::AWF_UNIVERSE); + EXPECT_EQ(architectureTypeFromString("awf/universe-"), ArchitectureType::AWF_UNIVERSE); + EXPECT_EQ(architectureTypeFromString("awf/universe "), ArchitectureType::AWF_UNIVERSE); + EXPECT_EQ(architectureTypeFromString(" awf/universe"), ArchitectureType::AWF_UNIVERSE); + EXPECT_EQ(architectureTypeFromString(" awf/universe "), ArchitectureType::AWF_UNIVERSE); + EXPECT_EQ(architectureTypeFromString("tier4/proposal"), ArchitectureType::TIER4_PROPOSAL); +} + +TEST(DataTypes, architectureTypeFromString_incorrect) +{ + EXPECT_THROW(architectureTypeFromString("unknown"), std::runtime_error); + EXPECT_THROW(architectureTypeFromString("awf/auto "), std::runtime_error); + EXPECT_THROW(architectureTypeFromString(" awf/auto"), std::runtime_error); + EXPECT_THROW(architectureTypeFromString("tier4/proposal "), std::runtime_error); + EXPECT_THROW(architectureTypeFromString(" tier4/proposal"), std::runtime_error); +} + +TEST(DataTypes, stringFromArchitectureType_correct) +{ + EXPECT_EQ(stringFromArchitectureType(ArchitectureType::AWF_AUTO), "awf/auto"); + EXPECT_EQ(stringFromArchitectureType(ArchitectureType::AWF_UNIVERSE), "awf/universe"); + EXPECT_EQ(stringFromArchitectureType(ArchitectureType::TIER4_PROPOSAL), "tier4/proposal"); +} + +TEST(DataTypes, stringFromArchitectureType_incorrect) +{ + EXPECT_THROW(stringFromArchitectureType(static_cast(-1)), std::runtime_error); + EXPECT_THROW(stringFromArchitectureType(static_cast(3)), std::runtime_error); + EXPECT_THROW(stringFromArchitectureType(static_cast(4)), std::runtime_error); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_ego_collision_metric.cpp b/test_runner/random_test_runner/test/test_ego_collision_metric.cpp new file mode 100644 index 00000000000..00348272ae5 --- /dev/null +++ b/test_runner/random_test_runner/test/test_ego_collision_metric.cpp @@ -0,0 +1,98 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +TEST(Metrics, EgoCollisionMetric_noCollision) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.4999)); +} + +TEST(Metrics, EgoCollisionMetric_collision) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.5001)); +} + +TEST(Metrics, EgoCollisionMetric_collisionOtherNPC) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("other", 0.1)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.5001)); +} + +TEST(Metrics, EgoCollisionMetric_noCollisionUpdate) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.1)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.5999)); +} + +TEST(Metrics, EgoCollisionMetric_noCollisionUpdateOtherNPC) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.1)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("other", 0.4)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.5999)); +} + +TEST(Metrics, EgoCollisionMetric_collisionUpdate) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.1)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.6001)); +} + +TEST(Metrics, EgoCollisionMetric_collisionUpdateOtherNPC) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.1)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("other", 0.4)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.6001)); +} + +TEST(Metrics, EgoCollisionMetric_collisionAfterLongTime) +{ + EgoCollisionMetric metric; + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 0.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 0.5)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 1.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 1.5)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 2.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 2.5)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 3.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 3.5)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 4.0)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 4.5)); + EXPECT_FALSE(metric.isThereEgosCollisionWith("npc", 5.0)); + EXPECT_TRUE(metric.isThereEgosCollisionWith("npc", 6.0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_goal_reached_metric.cpp b/test_runner/random_test_runner/test/test_goal_reached_metric.cpp new file mode 100644 index 00000000000..8b02235e024 --- /dev/null +++ b/test_runner/random_test_runner/test/test_goal_reached_metric.cpp @@ -0,0 +1,61 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +#include "test_utils.hpp" + +TEST(Metrics, GoalReachedMetric_noGoal) +{ + GoalReachedMetric metric; + EXPECT_FALSE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, 0.0))); +} + +TEST(Metrics, GoalReachedMetric_goalNotReached) +{ + GoalReachedMetric metric; + metric.setGoal(geometry_msgs::build() + .position(geometry_msgs::build().x(5.0).y(5.0).z(0.0)) + .orientation(geometry_msgs::msg::Quaternion())); + EXPECT_FALSE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, 0.0))); + EXPECT_FALSE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, 5.0))); + EXPECT_FALSE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, 0.0, 5.0))); + EXPECT_FALSE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, 5.0, 10.0))); + EXPECT_FALSE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, 10.0, 5.0))); +} + +TEST(Metrics, GoalReachedMetric_goalReached) +{ + GoalReachedMetric metric; + metric.setGoal(geometry_msgs::build() + .position(geometry_msgs::build().x(5.0).y(5.0).z(0.0)) + .orientation(geometry_msgs::msg::Quaternion())); + constexpr double a = std::sqrt(3.0) - 1e-3; + constexpr double less = 5.0 - a; + constexpr double more = 5.0 + a; + EXPECT_TRUE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, less, less))); + EXPECT_TRUE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, less, more))); + EXPECT_TRUE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, more, more))); + EXPECT_TRUE(metric.isGoalReached(getCanonicalizedEntityStatus(0.0, 0.0, more, less))); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_junit_xml_reporter.cpp b/test_runner/random_test_runner/test/test_junit_xml_reporter.cpp new file mode 100644 index 00000000000..969b2856eed --- /dev/null +++ b/test_runner/random_test_runner/test/test_junit_xml_reporter.cpp @@ -0,0 +1,293 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +#include "test_utils.hpp" + +std::string readFile(const std::string & path = "/tmp/result.junit.xml") +{ + std::ifstream file(path); + std::stringstream buffer; + buffer << file.rdbuf(); + return buffer.str(); +} + +TEST(JunitXmlReporter, initialize) +{ + EXPECT_NO_THROW(JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter"))); + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + EXPECT_NO_THROW(reporter.init("/tmp")); + EXPECT_NO_THROW(reporter.spawnTestCase("testsuite", "testcase")); +} + +TEST(JunitXmlReporter, reportCollision) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW( + testcase.reportCollision(makeNPCDescription("npc", 1.0, makeLaneletPose(123, 5.0)), 10.0)); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportCollision_double) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW( + testcase.reportCollision(makeNPCDescription("npc1", 1.0, makeLaneletPose(123, 5.0)), 10.0)); + EXPECT_NO_THROW( + testcase.reportCollision(makeNPCDescription("npc2", 2.0, makeLaneletPose(321, 7.0)), 17.0)); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportStandStill) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW(testcase.reportStandStill()); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportStandStill_double) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW(testcase.reportStandStill()); + EXPECT_NO_THROW(testcase.reportStandStill()); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportTimeout) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW(testcase.reportTimeout()); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportTimeout_double) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW(testcase.reportTimeout()); + EXPECT_NO_THROW(testcase.reportTimeout()); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportException) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW(testcase.reportException("exception_type", "Exception message")); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportException_double) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW(testcase.reportException("exception_type1", "Exception message 1")); + EXPECT_NO_THROW(testcase.reportException("exception_type2", "Exception message 2")); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportStandStillAndTimeout) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW(testcase.reportStandStill()); + EXPECT_NO_THROW(testcase.reportTimeout()); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +TEST(JunitXmlReporter, reportAll) +{ + JunitXmlReporter reporter(rclcpp::get_logger("test_junit_xml_reporter")); + reporter.init("/tmp"); + JunitXmlReporterTestCase testcase = reporter.spawnTestCase("testsuite", "testcase"); + + EXPECT_NO_THROW( + testcase.reportCollision(makeNPCDescription("npc", 1.0, makeLaneletPose(123, 5.0)), 10.0)); + EXPECT_NO_THROW(testcase.reportStandStill()); + EXPECT_NO_THROW(testcase.reportTimeout()); + EXPECT_NO_THROW(testcase.reportException("test_exception", "The vehicle has collided with npc")); + + EXPECT_NO_THROW(reporter.write()); + + const std::string report = readFile(); + const std::string ans = R"( + + + + + + + + + + +)"; + EXPECT_STREQ(report.c_str(), ans.c_str()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_lanelet_utils.cpp b/test_runner/random_test_runner/test/test_lanelet_utils.cpp new file mode 100644 index 00000000000..0171d21472c --- /dev/null +++ b/test_runner/random_test_runner/test/test_lanelet_utils.cpp @@ -0,0 +1,267 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-6; + +TEST(LaneletUtils, initialize) +{ + const std::string path = + ament_index_cpp::get_package_share_directory("random_test_runner") + "/map/lanelet2_map.osm"; + EXPECT_NO_THROW(LaneletUtils utils(path)); +} + +TEST(LaneletUtils, computeDistance) +{ + traffic_simulator_msgs::msg::LaneletPose from = makeLaneletPose(34621, 0.0), + to = makeLaneletPose(34621, 1.0); + EXPECT_NEAR(1.0, getLaneletUtils().computeDistance(from, to), EPS); +} + +TEST(LaneletUtils, computeDistance_reverse) +{ + traffic_simulator_msgs::msg::LaneletPose from = makeLaneletPose(34621, 21.3), + to = makeLaneletPose(34621, 17.7); + EXPECT_NEAR(getLaneletUtils().computeDistance(from, to), 3.6, EPS); +} + +TEST(LaneletUtils, computeDistance_differentLanelet) +{ + traffic_simulator_msgs::msg::LaneletPose from = makeLaneletPose(34594, 15.0), + to = makeLaneletPose(34621, 5.0); + EXPECT_NEAR(getLaneletUtils().computeDistance(from, to), 10.032117179351, 1e-2); +} + +TEST(LaneletUtils, getOppositeLanelet) +{ + traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 5.0); + std::optional opposite = + getLaneletUtils().getOppositeLaneLet(pose); + EXPECT_TRUE(opposite); + EXPECT_LANELET_POSE_NEAR(opposite.value(), makeLaneletPose(34981, 3.611), 1e-2); +} + +TEST(LaneletUtils, getOppositeLanelet_notExisting) +{ + traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(1, 0.0); + std::optional opposite = + getLaneletUtils().getOppositeLaneLet(pose); + EXPECT_FALSE(opposite); +} + +TEST(LaneletUtils, getLanesWithinDistance_incorrectDistances) +{ + traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 5.0); + EXPECT_THROW(getLaneletUtils().getLanesWithinDistance(pose, 1.0, 0.0), std::runtime_error); +} +#include +TEST(LaneletUtils, getLanesWithinDistance_straightRoad) +{ + traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34462, 15.0); + EXPECT_NO_THROW(getLaneletUtils().getLanesWithinDistance(pose, 0.0, 10.0)); + std::vector lanes = getLaneletUtils().getLanesWithinDistance(pose, 0.0, 10.0); + EXPECT_EQ(lanes.size(), size_t(4)); + EXPECT_EQ(lanes[0].lanelet_id, 34462); + EXPECT_EQ(lanes[1].lanelet_id, 34462); + EXPECT_EQ(lanes[2].lanelet_id, 34513); + EXPECT_EQ(lanes[3].lanelet_id, 34513); +} + +TEST(LaneletUtils, getLanesWithinDistance_intersection) +{ + traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 5.0); + EXPECT_NO_THROW(getLaneletUtils().getLanesWithinDistance(pose, 0.0, 10.0)); + std::vector lanes = getLaneletUtils().getLanesWithinDistance(pose, 0.0, 10.0); + EXPECT_EQ(lanes.size(), size_t(13)); + EXPECT_EQ(lanes[0].lanelet_id, 34585); + EXPECT_EQ(lanes[1].lanelet_id, 34594); + EXPECT_EQ(lanes[2].lanelet_id, 34621); + EXPECT_EQ(lanes[3].lanelet_id, 34621); + EXPECT_EQ(lanes[4].lanelet_id, 34636); + EXPECT_EQ(lanes[5].lanelet_id, 34642); + EXPECT_EQ(lanes[6].lanelet_id, 34645); + EXPECT_EQ(lanes[7].lanelet_id, 34651); + EXPECT_EQ(lanes[8].lanelet_id, 34976); + EXPECT_EQ(lanes[9].lanelet_id, 34981); + EXPECT_EQ(lanes[10].lanelet_id, 34981); + EXPECT_EQ(lanes[11].lanelet_id, 35001); + EXPECT_EQ(lanes[12].lanelet_id, 35051); +} + +TEST(LaneletUtils, getLaneletIds) +{ + EXPECT_NO_THROW(getLaneletUtils().getLaneletIds()); + const auto ids = getLaneletUtils().getLaneletIds(); + EXPECT_EQ(ids.size(), size_t(83)); + unsigned int idx = 0; + EXPECT_EQ(ids[idx++], 35051); + EXPECT_EQ(ids[idx++], 35036); + EXPECT_EQ(ids[idx++], 35001); + EXPECT_EQ(ids[idx++], 34850); + EXPECT_EQ(ids[idx++], 34786); + EXPECT_EQ(ids[idx++], 34783); + EXPECT_EQ(ids[idx++], 35031); + EXPECT_EQ(ids[idx++], 34777); + EXPECT_EQ(ids[idx++], 34774); + EXPECT_EQ(ids[idx++], 34768); + EXPECT_EQ(ids[idx++], 35016); + EXPECT_EQ(ids[idx++], 34762); + EXPECT_EQ(ids[idx++], 34603); + EXPECT_EQ(ids[idx++], 120659); + EXPECT_EQ(ids[idx++], 34426); + EXPECT_EQ(ids[idx++], 34591); + EXPECT_EQ(ids[idx++], 34795); + EXPECT_EQ(ids[idx++], 34414); + EXPECT_EQ(ids[idx++], 34579); + EXPECT_EQ(ids[idx++], 34756); + EXPECT_EQ(ids[idx++], 34576); + EXPECT_EQ(ids[idx++], 34780); + EXPECT_EQ(ids[idx++], 34399); + EXPECT_EQ(ids[idx++], 34753); + EXPECT_EQ(ids[idx++], 34570); + EXPECT_EQ(ids[idx++], 34564); + EXPECT_EQ(ids[idx++], 34741); + EXPECT_EQ(ids[idx++], 34976); + EXPECT_EQ(ids[idx++], 34468); + EXPECT_EQ(ids[idx++], 35026); + EXPECT_EQ(ids[idx++], 34645); + EXPECT_EQ(ids[idx++], 34465); + EXPECT_EQ(ids[idx++], 34642); + EXPECT_EQ(ids[idx++], 34585); + EXPECT_EQ(ids[idx++], 34789); + EXPECT_EQ(ids[idx++], 34408); + EXPECT_EQ(ids[idx++], 34498); + EXPECT_EQ(ids[idx++], 34675); + EXPECT_EQ(ids[idx++], 35046); + EXPECT_EQ(ids[idx++], 34792); + EXPECT_EQ(ids[idx++], 34411); + EXPECT_EQ(ids[idx++], 34392); + EXPECT_EQ(ids[idx++], 34510); + EXPECT_EQ(ids[idx++], 34438); + EXPECT_EQ(ids[idx++], 34615); + EXPECT_EQ(ids[idx++], 34495); + EXPECT_EQ(ids[idx++], 34672); + EXPECT_EQ(ids[idx++], 34621); + EXPECT_EQ(ids[idx++], 34594); + EXPECT_EQ(ids[idx++], 34507); + EXPECT_EQ(ids[idx++], 34684); + EXPECT_EQ(ids[idx++], 34420); + EXPECT_EQ(ids[idx++], 34423); + EXPECT_EQ(ids[idx++], 34981); + EXPECT_EQ(ids[idx++], 34600); + EXPECT_EQ(ids[idx++], 34462); + EXPECT_EQ(ids[idx++], 34385); + EXPECT_EQ(ids[idx++], 34639); + EXPECT_EQ(ids[idx++], 35021); + EXPECT_EQ(ids[idx++], 34513); + EXPECT_EQ(ids[idx++], 34690); + EXPECT_EQ(ids[idx++], 34441); + EXPECT_EQ(ids[idx++], 34606); + EXPECT_EQ(ids[idx++], 34624); + EXPECT_EQ(ids[idx++], 34630); + EXPECT_EQ(ids[idx++], 34633); + EXPECT_EQ(ids[idx++], 34636); + EXPECT_EQ(ids[idx++], 34648); + EXPECT_EQ(ids[idx++], 34651); + EXPECT_EQ(ids[idx++], 34654); + EXPECT_EQ(ids[idx++], 34666); + EXPECT_EQ(ids[idx++], 34678); + EXPECT_EQ(ids[idx++], 120660); + EXPECT_EQ(ids[idx++], 34681); + EXPECT_EQ(ids[idx++], 120545); + EXPECT_EQ(ids[idx++], 34693); + EXPECT_EQ(ids[idx++], 34696); + EXPECT_EQ(ids[idx++], 34705); + EXPECT_EQ(ids[idx++], 34708); + EXPECT_EQ(ids[idx++], 34744); + EXPECT_EQ(ids[idx++], 34750); + EXPECT_EQ(ids[idx++], 34378); + EXPECT_EQ(ids[idx++], 34759); +} + +TEST(LaneletUtils, toMapPose) +{ + const traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 10.0); + EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose)); + EXPECT_POSE_NEAR( + getLaneletUtils().toMapPose(pose).pose, + geometry_msgs::build() + .position(geometry_msgs::build() + .x(3747.3511648127) + .y(73735.0699484234) + .z(0.3034051453)) + .orientation(geometry_msgs::build() + .x(0.0) + .y(0.0) + .z(-0.9719821398) + .w(0.2350547166)), + EPS); +} + +TEST(LaneletUtils, getRoute_turn) +{ + EXPECT_NO_THROW(getLaneletUtils().getRoute(34681, 34513)); + const std::vector route = getLaneletUtils().getRoute(34681, 34513); + EXPECT_EQ(route.size(), size_t(3)); + EXPECT_EQ(route[0], 34681); + EXPECT_EQ(route[1], 34684); + EXPECT_EQ(route[2], 34513); +} + +TEST(LaneletUtils, getRoute_intersection) +{ + EXPECT_NO_THROW(getLaneletUtils().getRoute(34600, 34621)); + const std::vector route = getLaneletUtils().getRoute(34600, 34621); + EXPECT_EQ(route.size(), size_t(3)); + EXPECT_EQ(route[0], 34600); + EXPECT_EQ(route[1], 34594); + EXPECT_EQ(route[2], 34621); +} + +TEST(LaneletUtils, getLaneletLength) +{ + EXPECT_NEAR(getLaneletUtils().getLaneletLength(34621), 49.9125380565, EPS); + EXPECT_NEAR(getLaneletUtils().getLaneletLength(34507), 55.4273243719, EPS); + EXPECT_NEAR(getLaneletUtils().getLaneletLength(34570), 19.5778003979, EPS); +} + +TEST(LaneletUtils, isInLanelet) +{ + // lanelet 34621 + EXPECT_TRUE(getLaneletUtils().isInLanelet(34621, 10.0)); + EXPECT_FALSE(getLaneletUtils().isInLanelet(34621, -1.0)); + EXPECT_FALSE(getLaneletUtils().isInLanelet(34621, 50.0)); + // lanelet 34507 + EXPECT_TRUE(getLaneletUtils().isInLanelet(34507, 10.0)); + EXPECT_FALSE(getLaneletUtils().isInLanelet(34507, -1.0)); + EXPECT_FALSE(getLaneletUtils().isInLanelet(34507, 56.0)); + // lanelet 34570 + EXPECT_TRUE(getLaneletUtils().isInLanelet(34570, 10.0)); + EXPECT_FALSE(getLaneletUtils().isInLanelet(34570, -1.0)); + EXPECT_FALSE(getLaneletUtils().isInLanelet(34570, 20.0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_randomizers.cpp b/test_runner/random_test_runner/test/test_randomizers.cpp new file mode 100644 index 00000000000..171e0cf0c27 --- /dev/null +++ b/test_runner/random_test_runner/test/test_randomizers.cpp @@ -0,0 +1,129 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +#include "expect_eq_macros.hpp" + +TEST(UniformRandomizer, int_generate255) +{ + RandomizationEnginePtr engine = std::make_shared(); + UniformRandomizer rand(engine, 0, 255); + int sum = 0; + for (int i = 0; i < 1e+6; ++i) { + const auto generated = rand.generate(); + sum += generated; + EXPECT_IN_RANGE(generated, 0, 255); + } + EXPECT_NEAR(static_cast(sum) / 1e+6, 127.5, 1.0); +} + +TEST(UniformRandomizer, int_generate10) +{ + RandomizationEnginePtr engine = std::make_shared(); + UniformRandomizer rand(engine, 0, 10); + int sum = 0; + for (int i = 0; i < 1e+6; ++i) { + const auto generated = rand.generate(); + sum += generated; + EXPECT_IN_RANGE(generated, 0, 10); + } + EXPECT_NEAR(static_cast(sum) / 1e+6, 5.0, 0.1); +} + +TEST(UniformRandomizer, int_generate10seed) +{ + RandomizationEnginePtr engine = std::make_shared(1); + UniformRandomizer rand(engine, 0, 10); + EXPECT_EQ(rand.generate(), 4); + EXPECT_EQ(rand.generate(), 10); + EXPECT_EQ(rand.generate(), 7); + EXPECT_EQ(rand.generate(), 10); + EXPECT_EQ(rand.generate(), 0); +} + +TEST(UniformRandomizer, int_setRange10) +{ + RandomizationEnginePtr engine = std::make_shared(); + UniformRandomizer rand(engine, 0, 255); + rand.setRange(10, 20); + int sum = 0; + for (int i = 0; i < 1e+6; ++i) { + const auto generated = rand.generate(); + sum += generated; + EXPECT_IN_RANGE(generated, 10, 20); + } + EXPECT_NEAR(static_cast(sum) / 1e+6, 15.0, 0.1); +} + +TEST(UniformRandomizer, double_generate100) +{ + RandomizationEnginePtr engine = std::make_shared(); + UniformRandomizer rand(engine, 0.0, 100.0); + double sum = 0.0; + for (int i = 0; i < 1e+6; ++i) { + const auto generated = rand.generate(); + sum += generated; + EXPECT_IN_RANGE(generated, 0.0, 100.0); + } + EXPECT_NEAR(sum / 1e+6, 50.0, 1.0); +} + +TEST(UniformRandomizer, double_generate1) +{ + RandomizationEnginePtr engine = std::make_shared(); + UniformRandomizer rand(engine, 0.0, 1.0); + double sum = 0.0; + for (int i = 0; i < 1e+6; ++i) { + const auto generated = rand.generate(); + sum += generated; + EXPECT_IN_RANGE(generated, 0.0, 1.0); + } + EXPECT_NEAR(sum / 1e+6, 0.5, 0.1); +} + +TEST(UniformRandomizer, double_generate1seed) +{ + RandomizationEnginePtr engine = std::make_shared(1); + UniformRandomizer rand(engine, 0.0, 1.0); + EXPECT_NEAR(rand.generate(), 0.997185, 1e-6); + EXPECT_NEAR(rand.generate(), 0.932557, 1e-6); + EXPECT_NEAR(rand.generate(), 0.128124, 1e-6); + EXPECT_NEAR(rand.generate(), 0.999041, 1e-6); + EXPECT_NEAR(rand.generate(), 0.236089, 1e-6); +} + +TEST(UniformRandomizer, double_setRange1) +{ + RandomizationEnginePtr engine = std::make_shared(); + UniformRandomizer rand(engine, 0.0, 100.0); + rand.setRange(1.0, 2.0); + double sum = 0.0; + for (int i = 0; i < 1e+6; ++i) { + const auto generated = rand.generate(); + sum += generated; + EXPECT_IN_RANGE(generated, 1.0, 2.0); + } + EXPECT_NEAR(sum / 1e+6, 1.5, 0.1); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp new file mode 100644 index 00000000000..cfaee437843 --- /dev/null +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -0,0 +1,202 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +class MockFieldOperatorApplication +{ +public: + MOCK_METHOD(void, declare_parameter_mock, (), ()); + MOCK_METHOD(bool, engageable, (), ()); + MOCK_METHOD(void, engage, (), ()); + + template + void declare_parameter(std::string, bool) + { + if constexpr (std::is_same_v) { + declare_parameter_mock(); + return; + } + throw std::runtime_error("Unexpected typename"); + } +}; + +struct DummyCanonicalizedLaneletPose +{ +}; + +class MockTrafficSimulatorAPI +{ +public: + traffic_simulator::EntityStatus entity_status_; + std::shared_ptr<::testing::StrictMock> + field_operator_application_mock = + std::make_shared<::testing::StrictMock>(); + + MOCK_METHOD(bool, updateFrame, (), ()); + MOCK_METHOD( + bool, spawn, + (const std::string &, DummyCanonicalizedLaneletPose, + const traffic_simulator_msgs::msg::VehicleParameters &, const std::string &), + ()); + MOCK_METHOD( + void, setEntityStatus, + (const std::string &, DummyCanonicalizedLaneletPose, + const traffic_simulator_msgs::msg::ActionStatus), + ()); + MOCK_METHOD(void, attachLidarSensor, (const simulation_api_schema::LidarConfiguration &), ()); + MOCK_METHOD( + void, attachDetectionSensor, (const simulation_api_schema::DetectionSensorConfiguration &), ()); + MOCK_METHOD( + bool, attachOccupancyGridSensor, (simulation_api_schema::OccupancyGridSensorConfiguration), ()); + MOCK_METHOD(void, asFieldOperatorApplicationMock, (const std::string &), ()); + MOCK_METHOD( + void, requestAssignRoute, (const std::string &, std::vector), + ()); + MOCK_METHOD( + DummyCanonicalizedLaneletPose, canonicalize, (const traffic_simulator::LaneletPose &), ()); + MOCK_METHOD( + void, spawn, + (const std::string &, DummyCanonicalizedLaneletPose, + const traffic_simulator_msgs::msg::VehicleParameters &), + ()); + MOCK_METHOD(void, requestSpeedChange, (const std::string &, double, bool), ()); + MOCK_METHOD(bool, isEgoSpawned, (), ()); + MOCK_METHOD(bool, isNpcLogicStarted, (), ()); + MOCK_METHOD(void, startNpcLogic, (), ()); + MOCK_METHOD(bool, despawn, (const std::string), ()); + MOCK_METHOD(std::string, getEgoName, (), ()); + MOCK_METHOD(double, getCurrentTime, (), ()); + MOCK_METHOD(void, getEntityStatusMock, (const std::string &), ()); + MOCK_METHOD(bool, entityExists, (const std::string &), ()); + MOCK_METHOD(bool, checkCollision, (const std::string &, const std::string &), ()); + + ::testing::StrictMock & asFieldOperatorApplication( + const std::string & name) + { + asFieldOperatorApplicationMock(name); + return *field_operator_application_mock; + } + + 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); + } + + void setEntityStatusNecessaryValues( + double time, geometry_msgs::msg::Pose map_pose, geometry_msgs::msg::Twist twist) + { + entity_status_.time = time; + entity_status_.pose = map_pose; + entity_status_.action_status.twist = twist; + } +}; + +TEST(TestExecutor, InitializeWithNoNPCs) +{ + ::testing::Sequence sequence; + auto MockAPI = std::make_shared<::testing::StrictMock>(); + + auto test_case = common::junit::SimpleTestCase("test_case"); + auto test_executor = TestExecutor( + MockAPI, TestDescription(), JunitXmlReporterTestCase(test_case), + SimulatorType::SIMPLE_SENSOR_SIMULATOR, ArchitectureType::AWF_UNIVERSE, + rclcpp::get_logger("test_executor_test")); + + EXPECT_CALL(*MockAPI, updateFrame).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, canonicalize).Times(1).InSequence(sequence); + EXPECT_CALL( + *MockAPI, spawn( + ::testing::A(), ::testing::A(), + ::testing::A(), + ::testing::A())) + .Times(1) + .InSequence(sequence); + EXPECT_CALL(*MockAPI, canonicalize).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, setEntityStatus).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, attachLidarSensor).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, attachDetectionSensor).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, attachOccupancyGridSensor).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, asFieldOperatorApplicationMock).Times(1).InSequence(sequence); + EXPECT_CALL(*(MockAPI->field_operator_application_mock), declare_parameter_mock) + .Times(1) + .InSequence(sequence); + EXPECT_CALL(*MockAPI, canonicalize).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, requestAssignRoute).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, asFieldOperatorApplicationMock).Times(1).InSequence(sequence); + EXPECT_CALL(*(MockAPI->field_operator_application_mock), engage).Times(1).InSequence(sequence); + + test_executor.initialize(); +} + +TEST(TestExecutor, UpdateNoNPCs) +{ + ::testing::Sequence sequence; + auto MockAPI = std::make_shared<::testing::StrictMock>(); + + auto test_case = common::junit::SimpleTestCase("test_case"); + auto test_executor = TestExecutor( + MockAPI, TestDescription(), JunitXmlReporterTestCase(test_case), + SimulatorType::SIMPLE_SENSOR_SIMULATOR, ArchitectureType::AWF_UNIVERSE, + rclcpp::get_logger("test_executor_test")); + + EXPECT_CALL(*MockAPI, isEgoSpawned) + .Times(1) + .InSequence(sequence) + .WillOnce(::testing::Return(false)); + EXPECT_CALL(*MockAPI, isNpcLogicStarted) + .Times(1) + .InSequence(sequence) + .WillOnce(::testing::Return(false)); + EXPECT_CALL(*MockAPI, startNpcLogic).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, isEgoSpawned) + .Times(1) + .InSequence(sequence) + .WillOnce(::testing::Return(false)); + EXPECT_CALL(*MockAPI, getCurrentTime) + .Times(1) + .InSequence(sequence) + .WillOnce(::testing::Return(1.0)); + EXPECT_CALL(*MockAPI, getEntityStatusMock).Times(1).InSequence(sequence); + EXPECT_CALL(*MockAPI, updateFrame).Times(1).InSequence(sequence); + + test_executor.update(); +} + +TEST(TestExecutor, DeinitializeWithNoNPCs) +{ + auto MockAPI = std::make_shared<::testing::StrictMock>(); + auto test_case = common::junit::SimpleTestCase("test_case"); + auto test_executor = TestExecutor( + MockAPI, TestDescription(), JunitXmlReporterTestCase(test_case), + SimulatorType::SIMPLE_SENSOR_SIMULATOR, ArchitectureType::AWF_UNIVERSE, + rclcpp::get_logger("test_executor_test")); + + EXPECT_CALL(*MockAPI, despawn("ego")).Times(1); + + test_executor.deinitialize(); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleMock(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_test_randomizer.cpp b/test_runner/random_test_runner/test/test_test_randomizer.cpp new file mode 100644 index 00000000000..3110b21754b --- /dev/null +++ b/test_runner/random_test_runner/test/test_test_randomizer.cpp @@ -0,0 +1,165 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include +#include +#include + +#include "expect_eq_macros.hpp" +#include "test_utils.hpp" + +constexpr double EPS = 1e-6; + +TEST(TestRandomizer, initialize) +{ + TestSuiteParameters suite_params; + suite_params.ego_goal_lanelet_id = 1; + suite_params.ego_goal_partial_randomization = true; + TestCaseParameters case_params; + case_params.seed = 1; + TestRandomizer randomizer( + rclcpp::get_logger("test_randomizer"), suite_params, case_params, getLaneletUtilsPtr()); +} + +TEST(TestRandomizer, generate_10NPC) +{ + TestSuiteParameters suite_params; + suite_params.ego_goal_lanelet_id = 34621; + suite_params.ego_goal_partial_randomization = true; + TestCaseParameters case_params; + case_params.seed = 1; + rclcpp::Logger logger = rclcpp::get_logger("test_randomizer"); + logger.set_level(rclcpp::Logger::Level::Unset); + TestRandomizer randomizer(logger, suite_params, case_params, getLaneletUtilsPtr()); + + TestDescription description = randomizer.generate(); + // ego data + EXPECT_LANELET_POSE_NEAR( + description.ego_start_position, makeLaneletPose(34705, 6.2940393113), EPS); + EXPECT_LANELET_POSE_NEAR( + description.ego_goal_position, makeLaneletPose(34642, 9.6945373700), EPS); + EXPECT_POSE_NEAR( + description.ego_goal_pose, + geometry_msgs::build() + .position(geometry_msgs::build() + .x(3759.7298049304) + .y(73741.1526960728) + .z(0.0698702227)) + .orientation(geometry_msgs::build() + .x(0.0) + .y(0.0) + .z(-0.9854984261) + .w(0.1696845662)), + EPS); + // npc data + EXPECT_EQ(description.npcs_descriptions.size(), size_t(10)); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[0], + makeNPCDescription("npc0", 0.7308464889, makeLaneletPose(34624, 7.5543207194)), EPS) + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[1], + makeNPCDescription("npc1", 2.1743651011, makeLaneletPose(34642, 5.2207237465)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[2], + makeNPCDescription("npc2", 1.8113704071, makeLaneletPose(34786, 3.7184477027)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[3], + makeNPCDescription("npc3", 0.5684689900, makeLaneletPose(34615, 21.0757887426)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[4], + makeNPCDescription("npc4", 1.6430120231, makeLaneletPose(34684, 5.5834935067)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[5], + makeNPCDescription("npc5", 0.9952537087, makeLaneletPose(34690, 18.9720777813)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[6], + makeNPCDescription("npc6", 2.5068937586, makeLaneletPose(34762, 6.9548768523)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[7], + makeNPCDescription("npc7", 0.5976369610, makeLaneletPose(34976, 2.5577956602)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[8], + makeNPCDescription("npc8", 2.1763201035, makeLaneletPose(34621, 9.4524364223)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[9], + makeNPCDescription("npc9", 0.9938772341, makeLaneletPose(34708, 7.0635798983)), EPS); +} + +TEST(TestRandomizer, generate_8NPC) +{ + TestSuiteParameters suite_params; + suite_params.ego_goal_lanelet_id = 34468; + suite_params.ego_goal_partial_randomization = true; + suite_params.npcs_count = 8; + TestCaseParameters case_params; + case_params.seed = 1234; + TestRandomizer randomizer( + rclcpp::get_logger("test_randomizer"), suite_params, case_params, getLaneletUtilsPtr()); + + TestDescription description = randomizer.generate(); + // ego data + EXPECT_LANELET_POSE_NEAR( + description.ego_start_position, makeLaneletPose(34648, 11.3744011441), EPS); + EXPECT_LANELET_POSE_NEAR( + description.ego_goal_position, makeLaneletPose(34507, 55.3754803281), EPS); + EXPECT_POSE_NEAR( + description.ego_goal_pose, + geometry_msgs::build() + .position(geometry_msgs::build() + .x(3799.5102437521) + .y(73815.1830249432) + .z(-2.9975350010)) + .orientation(geometry_msgs::build() + .x(0.0) + .y(0.0) + .z(0.2345315792) + .w(0.9721085013)), + EPS); + // npc data + EXPECT_EQ(description.npcs_descriptions.size(), size_t(8)); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[0], + makeNPCDescription("npc0", 1.1814815242, makeLaneletPose(34648, 1.0719996393)), EPS) + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[1], + makeNPCDescription("npc1", 2.5379073352, makeLaneletPose(34441, 1.8085656480)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[2], + makeNPCDescription("npc2", 1.3945431898, makeLaneletPose(34783, 13.6066129043)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[3], + makeNPCDescription("npc3", 1.3275385670, makeLaneletPose(34411, 6.2502502346)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[4], + makeNPCDescription("npc4", 1.9029904729, makeLaneletPose(34606, 14.5456329040)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[5], + makeNPCDescription("npc5", 0.6884530926, makeLaneletPose(34507, 31.5435000028)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[6], + makeNPCDescription("npc6", 1.4844558761, makeLaneletPose(34783, 1.8243487931)), EPS); + EXPECT_NPC_DESCRIPTION_NEAR( + description.npcs_descriptions[7], + makeNPCDescription("npc7", 2.4718253580, makeLaneletPose(34603, 13.0118273897)), EPS); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/random_test_runner/test/test_utils.hpp b/test_runner/random_test_runner/test/test_utils.hpp new file mode 100644 index 00000000000..deadfd0053b --- /dev/null +++ b/test_runner/random_test_runner/test/test_utils.hpp @@ -0,0 +1,87 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include +#include +#include +#include + +traffic_simulator::CanonicalizedEntityStatus getCanonicalizedEntityStatus( + const double time, const double twist_x, const double x, const double y = 0.0, + const double z = 0.0, const std::string & name = "name") +{ + static const std::string path = + ament_index_cpp::get_package_share_directory("random_test_runner") + "/map/lanelet2_map.osm"; + static const auto origin = geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0); + static const auto hdmap_utils_ptr = std::make_shared(path, origin); + + traffic_simulator::EntityStatus entity_status; + entity_status.type.type = traffic_simulator_msgs::msg::EntityType::VEHICLE; + entity_status.subtype.value = traffic_simulator_msgs::msg::EntitySubtype::CAR; + entity_status.time = time; + entity_status.name = name; + + traffic_simulator_msgs::msg::ActionStatus action_status; + action_status.current_action = std::string("action"); + action_status.twist.linear = + geometry_msgs::build().x(twist_x).y(0.0).z(0.0); + action_status.accel = geometry_msgs::msg::Accel(); + entity_status.action_status = action_status; + 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); +} + +inline traffic_simulator_msgs::msg::LaneletPose makeLaneletPose( + const long int lane_id, const double s) +{ + traffic_simulator_msgs::msg::LaneletPose lanelet_pose; + lanelet_pose.lanelet_id = lane_id; + lanelet_pose.s = s; + return lanelet_pose; +} + +LaneletUtils & getLaneletUtils() +{ + static const std::string path = + ament_index_cpp::get_package_share_directory("random_test_runner") + "/map/lanelet2_map.osm"; + static LaneletUtils utils(path); + return utils; +} + +std::shared_ptr getLaneletUtilsPtr() +{ + static const std::string path = + ament_index_cpp::get_package_share_directory("random_test_runner") + "/map/lanelet2_map.osm"; + static const auto utils = std::make_shared(path); + return utils; +} + +NPCDescription makeNPCDescription( + const std::string name, const double speed, + const traffic_simulator_msgs::msg::LaneletPose lanelet_pose) +{ + NPCDescription description; + description.name = name; + description.speed = speed; + description.start_position = lanelet_pose; + return description; +} diff --git a/test_runner/random_test_runner/test/test_yaml_test_params_saver.cpp b/test_runner/random_test_runner/test/test_yaml_test_params_saver.cpp new file mode 100644 index 00000000000..bbd919d12c2 --- /dev/null +++ b/test_runner/random_test_runner/test/test_yaml_test_params_saver.cpp @@ -0,0 +1,435 @@ +// 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. +// +// Co-developed by TIER IV, Inc. and Robotec.AI sp. z o.o. + +#include + +#include + +#include "test_utils.hpp" + +std::string readFile(const std::string & path = "/tmp/result.yaml") +{ + std::ifstream file(path); + std::stringstream buffer; + buffer << file.rdbuf(); + return buffer.str(); +} + +void writeToFile(const std::string & data, const std::string & path = "/tmp/result.yaml") +{ + std::ofstream file; + file.open(path); + file << data; + file.close(); +} + +// TestSuiteParameters + +TEST(YamlTestParamsSaver, TestSuiteParameters_encode) +{ + YAML::convert convert; + auto rhs = TestSuiteParameters(); + auto node = convert.encode(rhs); + + EXPECT_STREQ(node["test_name"].as().c_str(), rhs.name.c_str()); + EXPECT_STREQ(node["map_name"].as().c_str(), rhs.map_name.c_str()); + EXPECT_DOUBLE_EQ(node["ego_goal_s"].as(), rhs.ego_goal_s); + EXPECT_EQ(node["ego_goal_lanelet_id"].as(), rhs.ego_goal_lanelet_id); + EXPECT_EQ(node["ego_goal_partial_randomization"].as(), rhs.ego_goal_partial_randomization); + EXPECT_DOUBLE_EQ( + node["ego_goal_partial_randomization_distance"].as(), + rhs.ego_goal_partial_randomization_distance); + EXPECT_EQ(node["npc_count"].as(), rhs.npcs_count); + EXPECT_DOUBLE_EQ(node["npc_min_speed"].as(), rhs.npc_min_speed); + EXPECT_DOUBLE_EQ(node["npc_max_speed"].as(), rhs.npc_max_speed); + EXPECT_DOUBLE_EQ( + node["npc_min_spawn_distance_from_ego"].as(), rhs.npc_min_spawn_distance_from_ego); + EXPECT_DOUBLE_EQ( + node["npc_max_spawn_distance_from_ego"].as(), rhs.npc_max_spawn_distance_from_ego); +} + +TEST(YamlTestParamsSaver, TestSuiteParameters_decodeMap) +{ + YAML::convert convert; + + auto rhs = TestSuiteParameters(); + auto node = YAML::Node(YAML::NodeType::Map); + + node["test_name"] = "random_test_1"; + node["map_name"] = "test_map_name"; + node["ego_goal_s"] = 15.5; + node["ego_goal_lanelet_id"] = 1600; + node["ego_goal_partial_randomization"] = true; + node["ego_goal_partial_randomization_distance"] = 100.0; + node["npc_count"] = 20; + node["npc_min_speed"] = 0.3; + node["npc_max_speed"] = 7.0; + node["npc_min_spawn_distance_from_ego"] = 10.1; + node["npc_max_spawn_distance_from_ego"] = 100.1; + + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, true); + EXPECT_STREQ(rhs.name.c_str(), node["test_name"].as().c_str()); + EXPECT_STREQ(rhs.map_name.c_str(), node["map_name"].as().c_str()); + EXPECT_DOUBLE_EQ(rhs.ego_goal_s, node["ego_goal_s"].as()); + EXPECT_EQ(rhs.ego_goal_lanelet_id, node["ego_goal_lanelet_id"].as()); + EXPECT_EQ(rhs.ego_goal_partial_randomization, node["ego_goal_partial_randomization"].as()); + EXPECT_DOUBLE_EQ( + rhs.ego_goal_partial_randomization_distance, + node["ego_goal_partial_randomization_distance"].as()); + EXPECT_EQ(rhs.npcs_count, node["npc_count"].as()); + EXPECT_DOUBLE_EQ(rhs.npc_min_speed, node["npc_min_speed"].as()); + EXPECT_DOUBLE_EQ(rhs.npc_max_speed, node["npc_max_speed"].as()); + EXPECT_DOUBLE_EQ( + rhs.npc_min_spawn_distance_from_ego, node["npc_min_spawn_distance_from_ego"].as()); + EXPECT_DOUBLE_EQ( + rhs.npc_max_spawn_distance_from_ego, node["npc_max_spawn_distance_from_ego"].as()); + + EXPECT_STREQ(rhs.name.c_str(), "random_test_1"); + EXPECT_STREQ(rhs.map_name.c_str(), "test_map_name"); + EXPECT_DOUBLE_EQ(rhs.ego_goal_s, 15.5); + EXPECT_EQ(rhs.ego_goal_lanelet_id, 1600); + EXPECT_EQ(rhs.ego_goal_partial_randomization, true); + EXPECT_DOUBLE_EQ(rhs.ego_goal_partial_randomization_distance, 100.0); + EXPECT_EQ(rhs.npcs_count, 20); + EXPECT_DOUBLE_EQ(rhs.npc_min_speed, 0.3); + EXPECT_DOUBLE_EQ(rhs.npc_max_speed, 7.0); + EXPECT_DOUBLE_EQ(rhs.npc_min_spawn_distance_from_ego, 10.1); + EXPECT_DOUBLE_EQ(rhs.npc_max_spawn_distance_from_ego, 100.1); +} + +TEST(YamlTestParamsSaver, TestSuiteParameters_decodeNull) +{ + YAML::convert convert; + + auto rhs = TestSuiteParameters(); + auto node = YAML::Node(YAML::NodeType::Null); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); +} + +TEST(YamlTestParamsSaver, TestSuiteParameters_decodeScalar) +{ + YAML::convert convert; + + auto rhs = TestSuiteParameters(); + auto node = YAML::Node(YAML::NodeType::Scalar); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); +} + +TEST(YamlTestParamsSaver, TestSuiteParameters_decodeSequence) +{ + YAML::convert convert; + + auto rhs = TestSuiteParameters(); + auto node = YAML::Node(YAML::NodeType::Sequence); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); +} + +TEST(YamlTestParamsSaver, TestSuiteParameters_decodeUndefined) +{ + YAML::convert convert; + + auto rhs = TestSuiteParameters(); + auto node = YAML::Node(YAML::NodeType::Undefined); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); +} + +TEST(YamlTestParamsSaver, TestSuiteParameters_decodeDefault) +{ + YAML::convert convert; + + auto rhs = TestSuiteParameters(); + auto node = YAML::Node(); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); +} + +// TestCaseParameters + +TEST(YamlTestParamsSaver, TestCaseParameters_encode) +{ + YAML::convert convert; + auto rhs = TestCaseParameters(); + auto node = convert.encode(rhs); + + EXPECT_EQ(node["seed"].as(), rhs.seed); +} + +TEST(YamlTestParamsSaver, TestCaseParameters_decodeMap) +{ + YAML::convert convert; + auto rhs = TestCaseParameters(); + auto node = YAML::Node(YAML::NodeType::Map); + + node["seed"] = 5; + + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, true); + EXPECT_EQ(rhs.seed, 5); +} + +TEST(YamlTestParamsSaver, TestCaseParameters_decodeNull) +{ + YAML::convert convert; + auto rhs = TestCaseParameters(); + rhs.seed = 10; + auto node = YAML::Node(YAML::NodeType::Null); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); + EXPECT_EQ(rhs.seed, 10); +} + +TEST(YamlTestParamsSaver, TestCaseParameters_decodeScalar) +{ + YAML::convert convert; + auto rhs = TestCaseParameters(); + rhs.seed = 10; + auto node = YAML::Node(YAML::NodeType::Scalar); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); + EXPECT_EQ(rhs.seed, 10); +} + +TEST(YamlTestParamsSaver, TestCaseParameters_decodeSequence) +{ + YAML::convert convert; + auto rhs = TestCaseParameters(); + rhs.seed = 10; + auto node = YAML::Node(YAML::NodeType::Sequence); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); + EXPECT_EQ(rhs.seed, 10); +} + +TEST(YamlTestParamsSaver, TestCaseParameters_decodeUndefined) +{ + YAML::convert convert; + auto rhs = TestCaseParameters(); + rhs.seed = 10; + auto node = YAML::Node(YAML::NodeType::Undefined); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); + EXPECT_EQ(rhs.seed, 10); +} + +TEST(YamlTestParamsSaver, TestCaseParameters_decodeDefault) +{ + YAML::convert convert; + auto rhs = TestCaseParameters(); + rhs.seed = 10; + auto node = YAML::Node(); + auto result = convert.decode(node, rhs); + + EXPECT_EQ(result, false); + EXPECT_EQ(rhs.seed, 10); +} + +TEST(YamlTestParamsSaver, YamlTestParamsIO_read) +{ + auto test_params_IO = YamlTestParamsIO(rclcpp::get_logger("yaml_test_params_saver"), "/tmp"); + std::string file_data = + "random_test:\n\ + test_name: random_test\n\ + map_name: kashiwanoha_map\n\ + ego_goal_s: 0\n\ + ego_goal_lanelet_id: -1\n\ + ego_goal_partial_randomization: false\n\ + ego_goal_partial_randomization_distance: 25\n\ + npc_count: 10\n\ + npc_min_speed: 0.5\n\ + npc_max_speed: 3\n\ + npc_min_spawn_distance_from_ego: 10\n\ + npc_max_spawn_distance_from_ego: 100\n\ + test_cases:\n\ + - seed: 121660883\n\ + - seed: 478515236\n\ + - seed: 3404285739\n\ + - seed: 1008004056\n\ + - seed: 2015958364"; + writeToFile(file_data); + auto result = test_params_IO.read(); + + EXPECT_STREQ(result.first.name.c_str(), "random_test"); + EXPECT_STREQ(result.first.map_name.c_str(), "kashiwanoha_map"); + EXPECT_DOUBLE_EQ(result.first.ego_goal_s, 0.0); + EXPECT_EQ(result.first.ego_goal_lanelet_id, -1); + EXPECT_FALSE(result.first.ego_goal_partial_randomization); + EXPECT_DOUBLE_EQ(result.first.ego_goal_partial_randomization_distance, 25.0); + EXPECT_EQ(result.first.npcs_count, 10); + EXPECT_DOUBLE_EQ(result.first.npc_min_speed, 0.5); + EXPECT_DOUBLE_EQ(result.first.npc_max_speed, 3.0); + EXPECT_DOUBLE_EQ(result.first.npc_min_spawn_distance_from_ego, 10.0); + EXPECT_DOUBLE_EQ(result.first.npc_max_spawn_distance_from_ego, 100.0); + + EXPECT_EQ(result.second[0].seed, 121660883); + EXPECT_EQ(result.second[1].seed, 478515236); + EXPECT_EQ(result.second[2].seed, 3404285739); + EXPECT_EQ(result.second[3].seed, 1008004056); + EXPECT_EQ(result.second[4].seed, 2015958364); +} + +TEST(YamlTestParamsSaver, YamlTestParamsIO_readTooManySuites) +{ + auto test_params_IO = YamlTestParamsIO(rclcpp::get_logger("yaml_test_params_saver"), "/tmp"); + std::string file_data = + "random_test1:\n\ + test_name: random_test\n\ + map_name: kashiwanoha_map\n\ + ego_goal_s: 0\n\ + ego_goal_lanelet_id: -1\n\ + ego_goal_partial_randomization: false\n\ + ego_goal_partial_randomization_distance: 25\n\ + npc_count: 10\n\ + npc_min_speed: 0.5\n\ + npc_max_speed: 3\n\ + npc_min_spawn_distance_from_ego: 10\n\ + npc_max_spawn_distance_from_ego: 100\n\ + test_cases:\n\ + - seed: 121660883\n\ + - seed: 478515236\n\ + - seed: 3404285739\n\ + - seed: 1008004056\n\ + - seed: 2015958364\n\ +random_test2:\n\ + test_name: random_test\n\ + map_name: kashiwanoha_map\n\ + ego_goal_s: 0\n\ + ego_goal_lanelet_id: -1\n\ + ego_goal_partial_randomization: false\n\ + ego_goal_partial_randomization_distance: 25\n\ + npc_count: 10\n\ + npc_min_speed: 0.5\n\ + npc_max_speed: 3\n\ + npc_min_spawn_distance_from_ego: 10\n\ + npc_max_spawn_distance_from_ego: 100\n\ + test_cases:\n\ + - seed: 121660883\n\ + - seed: 478515236\n\ + - seed: 3404285739\n\ + - seed: 1008004056\n\ + - seed: 2015958364"; + writeToFile(file_data); + + EXPECT_THROW(test_params_IO.read(), std::runtime_error); +} + +TEST(YamlTestParamsSaver, YamlTestParamsIO_writeSingle) +{ + auto test_params_IO = YamlTestParamsIO(rclcpp::get_logger("yaml_test_params_saver"), "/tmp"); + TestSuiteParameters suite_parameters; + TestCaseParameters test_case_parameters1; + TestCaseParameters test_case_parameters2; + test_case_parameters1.seed = 100; + test_case_parameters2.seed = 200; + test_params_IO.addTestSuite(suite_parameters, "Suite1"); + test_params_IO.addTestCase(test_case_parameters1, "Suite1"); + test_params_IO.addTestCase(test_case_parameters2, "Suite1"); + test_params_IO.write(); + + std::string expected_file_data = + "Suite1:\n\ + test_name: random_test\n\ + map_name: kashiwanoha_map\n\ + ego_goal_s: 0\n\ + ego_goal_lanelet_id: -1\n\ + ego_goal_partial_randomization: false\n\ + ego_goal_partial_randomization_distance: 30\n\ + npc_count: 10\n\ + npc_min_speed: 0.5\n\ + npc_max_speed: 3\n\ + npc_min_spawn_distance_from_ego: 10\n\ + npc_max_spawn_distance_from_ego: 100\n\ + test_cases:\n\ + - seed: 100\n\ + - seed: 200"; + + EXPECT_STREQ(readFile().c_str(), expected_file_data.c_str()); +} + +TEST(YamlTestParamsSaver, YamlTestParamsIO_writeMultiple) +{ + auto test_params_IO = YamlTestParamsIO(rclcpp::get_logger("yaml_test_params_saver"), "/tmp"); + TestSuiteParameters suite_parameters1; + TestSuiteParameters suite_parameters2; + + suite_parameters1.name = "random_test1"; + suite_parameters2.name = "random_test2"; + + TestCaseParameters test_case_parameters1; + TestCaseParameters test_case_parameters2; + test_case_parameters1.seed = 100; + test_case_parameters2.seed = 200; + test_params_IO.addTestSuite(suite_parameters1, "Suite1"); + test_params_IO.addTestSuite(suite_parameters2, "Suite2"); + + test_params_IO.addTestCase(test_case_parameters1, "Suite1"); + test_params_IO.addTestCase(test_case_parameters2, "Suite2"); + + test_params_IO.write(); + + std::string expected_file_data = + "Suite1:\n\ + test_name: random_test1\n\ + map_name: kashiwanoha_map\n\ + ego_goal_s: 0\n\ + ego_goal_lanelet_id: -1\n\ + ego_goal_partial_randomization: false\n\ + ego_goal_partial_randomization_distance: 30\n\ + npc_count: 10\n\ + npc_min_speed: 0.5\n\ + npc_max_speed: 3\n\ + npc_min_spawn_distance_from_ego: 10\n\ + npc_max_spawn_distance_from_ego: 100\n\ + test_cases:\n\ + - seed: 100\n\ +Suite2:\n\ + test_name: random_test2\n\ + map_name: kashiwanoha_map\n\ + ego_goal_s: 0\n\ + ego_goal_lanelet_id: -1\n\ + ego_goal_partial_randomization: false\n\ + ego_goal_partial_randomization_distance: 30\n\ + npc_count: 10\n\ + npc_min_speed: 0.5\n\ + npc_max_speed: 3\n\ + npc_min_spawn_distance_from_ego: 10\n\ + npc_max_spawn_distance_from_ego: 100\n\ + test_cases:\n\ + - seed: 200"; + + EXPECT_STREQ(readFile().c_str(), expected_file_data.c_str()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 26ce2ab262f..7dc45b0622d 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -2,6 +2,161 @@ Changelog for package scenario_test_runner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.0 (2024-02-14) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/new_release_flow +* Merge pull request `#1077 `_ from tier4/fix/autoware-shutdown + Fix/autoware shutdown +* Merge branch 'master' into fix/autoware-shutdown +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into feature/consider_tread_in_ego_entity +* Merge pull request `#1150 `_ from tier4/feature/real-time-factor-control + Feature/real time factor control +* Merge remote-tracking branch 'tier/master' into feature/real-time-factor-control +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge pull request `#1159 `_ from tier4/revert/1096 + Revert/1096 +* Changes after review +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into feature/real-time-factor-control +* use_sim_time for openscenario_interpreter is parameterized and False by default +* Merge branch 'tier4:master' into random-test-runner-docs-update +* Revert "chore: add example scenario of DeleteEntityAction" + This reverts commit 8c6f8a498a226c2c35a9bedc18c90a95b5888f2b. +* Revert "chore: update author of sample scenario" + This reverts commit 604127d61e0a3014e36f8b05af2c52ab44cd99ea. +* Setting concealer use_sim_time manually instead of using global arguments. +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Global real time factor set with launch argument fix +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Merge remote-tracking branch 'origin/master' into fix/autoware-shutdown +* Update `shutdownAutoware` to respect the parameter `sigterm_timeout` +* Contributors: Kotaro Yoshimoto, Masaya Kataoka, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, pawellech1, yamacir-kit + +0.9.0 (2023-12-21) +------------------ +* Merge branch 'master' of https://github.com/tier4/scenario_simulator_v2 into prepare/release-0.9.0 +* Merge pull request `#1129 `_ from tier4/feature/RJD-716_add_follow_waypoint_controller +* Merge remote-tracking branch 'tier4/master' into fix/geometry-bug-fixes +* Merge pull request `#1149 `_ from tier4/feature/traffic-lights-awsim-support +* Merge remote-tracking branch 'origin/master' into feature/traffic-lights-awsim-support +* fixed typo in scenario name +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge pull request `#1033 `_ from tier4/feat/condition_groups_visualization +* add scenario file for varification +* Merge branch 'master' into feature/RJD-716_add_follow_waypoint_controller +* Merge branch 'feature/random_scenario' of https://github.com/tier4/scenario_simulator_v2 into feature/random_scenario +* Merge remote-tracking branch 'origin/master' into feature/random_scenario +* v2i and conventional traffic lights AWSIM scenario samples +* Merge branch 'experimental/merge-master' into feature/test-geometry-spline-subspline +* ref(follow_trajectory): remove unecessary changes +* Merge pull request `#1126 `_ from tier4/fix/duplicated_nodes +* feat(follow_trajectory): add follow waypoint controller +* fix(launch): add comment about the change from LifecycleNode to Node +* Merge branch 'master' into fix/duplicated_nodes +* Merge pull request `#1111 `_ from tier4/feature/traffic_light_confidence +* Merge remote-tracking branch 'tier4/master' into experimental/merge-master +* fix(launch): remove name keyword from openscenario_interpreter, openscenario_prepreocessor and simples_sensor_simulator nodes +* Merge remote-tracking branch 'origin/master' into feature/traffic_light_confidence +* Merge pull request `#1121 `_ from tier4/fix/sign-of-relative-distance +* Merge remote-tracking branch 'origin/master' into fix/sign-of-relative-distance +* Merge pull request `#1096 `_ from tier4/feature/deleted-entity +* Update test scenario to not expect negative distance for `RelativeDistanceCondition` in non-lane coordinate systems +* feat: update CustomCommandAction.PseudoTrafficSignalDetectorConfidenceSetAction@v1.yaml +* chore: update author of sample scenario +* chore: add example scenario of DeleteEntityAction +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into fix/rtc_command_action/continuous_execution +* Merge pull request `#1092 `_ from tier4/feature/control_rtc_auto_mode +* Merge pull request `#1099 `_ from tier4/pzyskowski/660/ss2-awsim-connection +* Merge pull request `#1098 `_ from tier4/fix/port_document +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* fix: destination of CustomCommandAction.RequestToCooperateCommandAction@v1.yaml +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1093 `_ from tier4/feature/RJD-614_follow_trajectory_action_pedestrian_cyclist_support +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* feat: add continuous rtc action execution in CustomCommandAction.RequestToCooperateCommandAction@v1.yaml +* refactor: delete cooperator property from CustomCommandAction.RequestToCooperateCommandAction@v1.yaml +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* change default port to 5555 +* Merge remote-tracking branch 'origin/master' into fix/port_document +* Merge remote-tracking branch 'origin/feature/control_rtc_auto_mode' into fix/rtc_command_action/continuous_execution +* refactor(simulator_core): use featureIdentifiersRequiringExternalPermissionForAutonomousDecisions instead of manualModules +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* Merge pull request `#1075 `_ from tier4/feature/RJD-96_detail_message_scenario_failure +* change default port to 8080 +* Merge remote-tracking branch 'origin' into feature/RJD-96_detail_message_scenario_failure +* change port +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* Merge branch 'master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge pull request `#1095 `_ from tier4/feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge branch 'master' into feature/freespace-distance-condition +* Merge remote-tracking branch 'origin/master' into pzyskowski/660/ss2-awsim-connection +* code refactor +* implement freespace for relative distance condition +* Init working version of DistanceCondition freespace +* Merge remote-tracking branch 'origin/master' into feature/control_rtc_auto_mode +* feat: update scenario for new RTC feature +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* feat(test_runner): add FollowTrajectoryAction for bicycle and pedestrian +* Merge remote-tracking branch 'origin/master' into AJD-805/baseline_update_rebased +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* fix(test_runner): fix scenarios directory +* feat(scenario_test_runner): add workflow for simulation failure - detailed message +* Merge pull request `#1069 `_ from tier4/feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/lanelet2_matching_via_rosdep +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/drop_galactic_support +* Merge pull request `#1027 `_ from tier4/feature/new_traffic_light +* Merge branch 'master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Initial version of freespace distance condition +* chore: change architecture_type to awf/universe/20230906 +* misc object updated in sample awsim +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* updated npc in awsim_sample scenario +* Update `update` to use `updateStatus` instead of `setStatus` +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/traffic_simulator/follow-trajectory-action-3 +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* added MiscObject to awsim_sample scenario +* fixed map path +* added modifier to awsim sample scenario +* sample awsim scenario added, model3d added to sample vehicle catalogue +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Add new test scenario `RoutingAction.FollowTrajectoryAction-autoware` +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* refactor: update architecture_type format +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/RJD-96_detail_message_scenario_failure +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* feat: add new architecture_type awf/universe/2023.08 +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/new_traffic_light +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/fallback_spline_to_line_segments +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Merge remote-tracking branch 'origin/master' into feature/allow-goal-modification +* Contributors: Dawid Moszyński, Kotaro Yoshimoto, Lukasz Chojnacki, Masaya Kataoka, Mateusz Palczuk, Michał Kiełczykowski, Paweł Lech, Piotr Zyskowski, Tatsuya Yamasaki, f0reachARR, kyoichi-sugahara, yamacir-kit + 0.8.0 (2023-09-05) ------------------ * Merge remote-tracking branch 'origin/master' into ref/RJD-553_restore_repeated_update_entity_status 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 047b9515896..ae0b1e34b43 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 @@ -77,6 +77,7 @@ def launch_setup(context, *args, **kwargs): 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="") workflow = LaunchConfiguration("workflow", default=Path("/dev/null")) # fmt: on @@ -97,6 +98,7 @@ def launch_setup(context, *args, **kwargs): 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"workflow := {workflow.perform(context)}") @@ -111,11 +113,12 @@ def make_parameters(): {"record": record}, {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, + {"sigterm_timeout": sigterm_timeout}, {"vehicle_model": vehicle_model}, ] parameters += make_vehicle_parameters() return parameters - + def make_vehicle_parameters(): parameters = [] @@ -144,6 +147,7 @@ def description(): 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("workflow", default_value=workflow ), # fmt: on @@ -171,7 +175,7 @@ def description(): namespace="simulation", output="screen", on_exit=ShutdownOnce(), - parameters=[{"port": port}]+make_vehicle_parameters(), + parameters=make_parameters() + [{"use_sim_time": True}], condition=IfCondition(launch_simple_sensor_simulator), ), # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear. @@ -184,7 +188,7 @@ def description(): executable="openscenario_interpreter_node", namespace="simulation", output="screen", - parameters=make_parameters(), + parameters=[{"use_sim_time": use_sim_time}]+make_parameters(), on_exit=ShutdownOnce(), ), Node( diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 63c314583fe..87f285a31b6 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 0.8.0 + 1.0.0 scenario test runner package Tatsuya Yamasaki Apache License 2.0 diff --git a/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-straight.yaml b/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-straight.yaml index bcb48868904..f28bc510e3d 100644 --- a/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-straight.yaml +++ b/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-straight.yaml @@ -96,7 +96,7 @@ OpenSCENARIO: s: 10 offset: 0 Orientation: *ORIENTATION - - time: 10 + - time: 12 Position: &ego_destination LanePosition: roadId: '' diff --git a/test_runner/scenario_test_runner/scenario/Visualization.simulation.context.yaml b/test_runner/scenario_test_runner/scenario/Visualization.simulation.context.yaml new file mode 100644 index 00000000000..61728cbbd72 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/Visualization.simulation.context.yaml @@ -0,0 +1,243 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 1 + date: "2023-07-06T08:32:04.803Z" + description: "" + author: Kyoichi Sugahara + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + CatalogLocation: [] + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: ego + Vehicle: + name: "" + vehicleCategory: car + BoundingBox: + Center: + x: 1.355 + y: 0 + z: 1.25 + Dimensions: + width: 1.83 + length: 4.77 + height: 2.5 + Performance: + maxSpeed: 50 + maxAcceleration: INF + maxDeceleration: INF + Axles: + FrontAxle: + maxSteering: 0.5236 + wheelDiameter: 0.78 + trackWidth: 1.63 + positionX: 1.385 + positionZ: 0.39 + RearAxle: + maxSteering: 0.5236 + wheelDiameter: 0.78 + trackWidth: 1.63 + positionX: -1.355 + positionZ: 0.39 + Properties: + Property: [] + ObjectController: + Controller: + name: '' + Properties: + Property: + - { name: isEgo, value: 'true' } + - name: Npc1 + Vehicle: + name: "" + vehicleCategory: car + BoundingBox: + Center: + x: 0 + y: 0 + z: 1.25 + Dimensions: + width: 1.8 + length: 4 + height: 2.5 + Performance: + maxSpeed: 50 + maxAcceleration: INF + maxDeceleration: INF + Axles: + FrontAxle: + maxSteering: 0.5236 + wheelDiameter: 0.6 + trackWidth: 1.8 + positionX: 2 + positionZ: 0.3 + RearAxle: + maxSteering: 0.5236 + wheelDiameter: 0.6 + trackWidth: 1.8 + positionX: 0 + positionZ: 0.3 + Properties: + Property: [] + ObjectController: + Controller: + name: "" + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: "" + laneId: "34513" + s: 1 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: + LanePosition: + roadId: "" + laneId: "34606" + s: 20 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - entityRef: Npc1 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: "" + laneId: "34513" + s: 30 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + - ControllerAction: + AssignControllerAction: + Controller: + name: "" + Properties: + Property: + - name: maxSpeed + value: "5" + Story: + - name: "" + Act: + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: "" + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: "" + Event: + - name: "EndCondition" + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: + LanePosition: + roadId: "" + laneId: "34606" + s: 20 + offset: 0 + Orientation: + type: relative + h: 0 + p: 0 + r: 0 + tolerance: 1 + - name: "" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 180 + rule: greaterThan + Action: + - name: "" + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: "" + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 180 + rule: greaterThan + - Condition: + - name: "" + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + CollisionCondition: + EntityRef: + entityRef: Npc1 + 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/delete.yaml b/test_runner/scenario_test_runner/scenario/delete.yaml deleted file mode 100644 index 8d798ce824b..00000000000 --- a/test_runner/scenario_test_runner/scenario/delete.yaml +++ /dev/null @@ -1,350 +0,0 @@ -OpenSCENARIO: - FileHeader: - author: "Kotaro Yoshimoto" - date: "2022-12-09T18:16:53+09:00" - description: "Scenario with DeleteEntityAction" - revMajor: 1 - revMinor: 0 - ParameterDeclarations: - ParameterDeclaration: - - name: random_offset - parameterType: double - value: 0 - ConstraintGroup: - - ValueConstraint: - - rule: lessOrEqual - value: 1.0 - - rule: greaterOrEqual - value: -1.0 - - CatalogLocations: - CatalogLocation: [] - RoadNetwork: - LogicFile: - filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map - Entities: - ScenarioObject: - - name: ego - Vehicle: - name: "" - vehicleCategory: car - ParameterDeclarations: - ParameterDeclaration: - # https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/simulator_model.param.yaml - - { name: vel_lim, parameterType: double, value: 50.0 } - - { name: vel_rate_lim, parameterType: double, value: 7.0 } - - { name: steer_lim, parameterType: double, value: 1.0 } - # https://github.com/autowarefoundation/sample_vehicle_launch/blob/main/sample_vehicle_description/config/vehicle_info.param.yaml - - { name: wheel_radius, parameterType: double, value: 0.39 } - - { name: wheel_width, parameterType: double, value: 0.42 } - - { name: wheel_base, parameterType: double, value: 2.74 } - - { name: wheel_tread, parameterType: double, value: 1.63 } - - { name: front_overhang, parameterType: double, value: 1.0 } - - { name: rear_overhang, parameterType: double, value: 1.03 } - - { name: left_overhang, parameterType: double, value: 0.1 } - - { name: right_overhang, parameterType: double, value: 0.1 } - - { name: vehicle_height, parameterType: double, value: 2.5 } - BoundingBox: - Center: - x: ${ ($front_overhang + $wheel_base + $rear_overhang) / 2 - $rear_overhang } - y: 0 - z: ${ $vehicle_height / 2 } - Dimensions: - width: ${ $left_overhang + $wheel_tread + $right_overhang } - length: ${ $front_overhang + $wheel_base + $rear_overhang } - height: $vehicle_height - Performance: - maxSpeed: $vel_lim - maxAcceleration: $vel_rate_lim - maxDeceleration: $vel_rate_lim - Axles: - FrontAxle: - maxSteering: $steer_lim - wheelDiameter: ${ 2 * $wheel_radius } - trackWidth: $wheel_tread - positionX: $wheel_base - positionZ: $wheel_radius - RearAxle: - maxSteering: 0 - wheelDiameter: ${ 2 * $wheel_radius } - trackWidth: $wheel_tread - positionX: 0 - positionZ: $wheel_radius - Properties: - Property: [] - ObjectController: - Controller: - name: "" - Properties: - Property: - - { name: isEgo, value: true } - - { name: maxJerk, value: 1.5 } - - { name: minJerk, value: -1.5 } - - name: Npc1 - Vehicle: - name: "" - 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: Npc2 - Vehicle: - name: "" - 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: [] - Storyboard: - Init: - Actions: - Private: - - entityRef: ego - PrivateAction: - - TeleportAction: - Position: - LanePosition: - roadId: "" - laneId: 34513 - s: 1 - offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 - - RoutingAction: - AcquirePositionAction: - Position: - 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 - - entityRef: Npc1 - PrivateAction: - - TeleportAction: - Position: - LanePosition: - roadId: "" - laneId: 34462 - s: 1 - offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 - Story: - - name: "" - Act: - - 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: ego.currentState - rule: equalTo - value: ARRIVED_GOAL - Action: - - name: "" - UserDefinedAction: - CustomCommandAction: - type: exitSuccess - StartTrigger: - ConditionGroup: - - Condition: - - name: "" - delay: 0 - conditionEdge: none - ByValueCondition: - SimulationTimeCondition: - value: 0 - rule: greaterThan - - name: "spawn" - ManeuverGroup: - - maximumExecutionCount: 1 - name: "" - Actors: - selectTriggeringEntities: false - Maneuver: - - name: "" - Event: - - name: "spawn" - priority: parallel - Action: - - name: "" - GlobalAction: - - EntityAction: - entityRef: Npc2 - AddEntityAction: - Position: - LanePosition: - roadId: "" - laneId: 34462 - s: 5 - offset: 0 - Orientation: - type: relative - h: 0 - p: 0 - r: 0 - StartTrigger: - ConditionGroup: - - Condition: - - name: "" - delay: 0 - conditionEdge: none - ByValueCondition: - SimulationTimeCondition: - value: 5 - rule: greaterThan - - name: "despawn" - ManeuverGroup: - - maximumExecutionCount: 1 - name: "" - Actors: - selectTriggeringEntities: false - Maneuver: - - name: "" - Event: - - name: "despawn" - priority: parallel - Action: - - name: "" - GlobalAction: - - EntityAction: - entityRef: Npc2 - DeleteEntityAction: - StartTrigger: - ConditionGroup: - - Condition: - - name: "" - delay: 0 - conditionEdge: none - ByValueCondition: - SimulationTimeCondition: - value: 7 - rule: greaterThan - StopTrigger: - ConditionGroup: - - Condition: - - name: (when (collide? ego npc) ...) - delay: 15 - conditionEdge: none - ByEntityCondition: - TriggeringEntities: - triggeringEntitiesRule: any - EntityRef: [entityRef: ego] - EntityCondition: - CollisionCondition: - EntityRef: { entityRef: Npc2 } diff --git a/test_runner/scenario_test_runner/scenario/sample_awsim_conventional_traffic_lights.yaml b/test_runner/scenario_test_runner/scenario/sample_awsim_conventional_traffic_lights.yaml new file mode 100644 index 00000000000..6ce04840e3a --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/sample_awsim_conventional_traffic_lights.yaml @@ -0,0 +1,234 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 1 + date: '2023-12-01T09:06:49.530Z' + description: + author: 'Piotr Zyskowski (last modified by: Piotr Zyskowski)' + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share shinjuku_map)/map + TrafficSignals: + TrafficSignalController: + - name: Intersection + Phase: + - name: StopSignal + duration: INF + TrafficSignalState: + - trafficSignalId: '1457' + state: red solidOn circle + - name: GoSignal + duration: INF + TrafficSignalState: + - trafficSignalId: '1457' + state: green solidOn circle + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: + - name: isEgo + value: 'true' + Storyboard: + Init: + Actions: + GlobalAction: + - InfrastructureAction: + TrafficSignalAction: + TrafficSignalControllerAction: + trafficSignalControllerRef: Intersection + phase: StopSignal + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '199' + s: 60 + offset: 0 + Orientation: + type: relative + h: -0.014 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: + LanePosition: + roadId: '' + laneId: '179' + s: 18.7918 + offset: -0.0708 + Orientation: + type: relative + h: -0.0246 + p: 0 + r: 0 + Story: + - name: '' + Act: + - name: switch_traffic_light_to_green_after_ego_stops_before_traffic_light_stop_line + ManeuverGroup: + - maximumExecutionCount: 1 + name: switch_traffic_light_to_green + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: event_change_to_green + priority: parallel + Action: + - name: '' + GlobalAction: + InfrastructureAction: + TrafficSignalAction: + TrafficSignalControllerAction: + trafficSignalControllerRef: Intersection + phase: GoSignal + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: sticky + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StartTrigger: + ConditionGroup: + - Condition: + - name: ego_is_in_position_before_the_traffic_light_stop_line + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: + LanePosition: + roadId: '' + laneId: '203' + s: 26.5 + offset: 0 + Orientation: + type: relative + h: -0.3695 + p: 0 + r: 0 + tolerance: 2 + - name: ego_stooped_for_a_moment + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + StandStillCondition: + duration: 5 + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: ego_reached_the_goal + delay: 0 + conditionEdge: sticky + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: + LanePosition: + roadId: '' + laneId: '179' + s: 18.7918 + offset: -0.0708 + Orientation: + type: relative + h: -0.0246 + p: 0 + r: 0 + tolerance: 0.5 + - name: ego_stopped_on_the_traffic_light + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + state: completeState + storyboardElementType: act + storyboardElementRef: switch_traffic_light_to_green_after_ego_stops_before_traffic_light_stop_line + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: timeout + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 90 + 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/sample_awsim_v2i_traffic_lights.yaml b/test_runner/scenario_test_runner/scenario/sample_awsim_v2i_traffic_lights.yaml new file mode 100644 index 00000000000..1b516d88fc9 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/sample_awsim_v2i_traffic_lights.yaml @@ -0,0 +1,220 @@ +ScenarioModifiers: + ScenarioModifier: [] +OpenSCENARIO: + FileHeader: + revMajor: 1 + revMinor: 1 + date: '2023-12-01T09:06:49.530Z' + description: + author: 'Piotr Zyskowski (last modified by: Piotr Zyskowski)' + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share shinjuku_map)/map + TrafficSignals: + TrafficSignalController: [] + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: + - name: isEgo + value: 'true' + Storyboard: + Init: + Actions: + UserDefinedAction: + CustomCommandAction: + type: >- + V2ITrafficSignalStateAction(1457, "red + solidOn circle") + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '199' + s: 60 + offset: 0 + Orientation: + type: relative + h: -0.014 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: + LanePosition: + roadId: '' + laneId: '179' + s: 18.7918 + offset: -0.0708 + Orientation: + type: relative + h: -0.0246 + p: 0 + r: 0 + Story: + - name: '' + Act: + - name: switch_traffic_light_to_green_after_ego_stops_before_traffic_light_stop_line + ManeuverGroup: + - maximumExecutionCount: 1 + name: switch_traffic_light_to_green + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: event_change_to_green + priority: parallel + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: >- + V2ITrafficSignalStateAction(1457, "green + solidOn circle") + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: sticky + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StartTrigger: + ConditionGroup: + - Condition: + - name: ego_is_in_position_before_the_traffic_light_stop_line + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: + LanePosition: + roadId: '' + laneId: '203' + s: 26.5 + offset: 0 + Orientation: + type: relative + h: -0.3695 + p: 0 + r: 0 + tolerance: 2 + - name: ego_stooped_for_a_moment + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + StandStillCondition: + duration: 5 + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: ego_reached_the_goal + delay: 0 + conditionEdge: sticky + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: + LanePosition: + roadId: '' + laneId: '179' + s: 18.7918 + offset: -0.0708 + Orientation: + type: relative + h: -0.0246 + p: 0 + r: 0 + tolerance: 0.5 + - name: ego_stopped_on_the_traffic_light + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + state: completeState + storyboardElementType: act + storyboardElementRef: switch_traffic_light_to_green_after_ego_stops_before_traffic_light_stop_line + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: timeout + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 90 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: []