diff --git a/.github/workflows/BuildAndRun.yaml b/.github/workflows/BuildAndRun.yaml index b0863d6d99e..6de471f0fe8 100644 --- a/.github/workflows/BuildAndRun.yaml +++ b/.github/workflows/BuildAndRun.yaml @@ -26,7 +26,6 @@ jobs: name: BuildAndRun runs-on: ${{ matrix.runs_on }} timeout-minutes: 180 - container: ros:${{ matrix.rosdistro }} env: DEBIAN_FRONTEND: noninteractive strategy: @@ -36,6 +35,11 @@ jobs: runs_on: [ubuntu-22.04, ubuntu-22.04-arm] cmake_build_type: [RelWithDebInfo, Release] # Debug build type is currently unavailable. @TODO Fix problem and add Debug build. steps: + - name: free disk + uses: jlumbroso/free-disk-space@main + with: + tool-cache: false + - name: Suppress warnings run: git config --global --add safe.directory '*' @@ -51,7 +55,24 @@ jobs: with: repository: RobotecAI/scenario_simulator_v2_scenarios path: src/scenario_simulator_v2_scenarios - + + - name: Cache apt and rosdep + id: cache-apt-rosdep + uses: actions/cache@v4 + with: + path: | + /var/cache/apt/archives + /var/lib/apt/lists + /etc/ros/rosdep + key: ${{ runner.os }}-ros-${{ matrix.rosdistro }}-apt-${{ hashFiles('**/package.xml', '**/dependency_*.repos') }} + restore-keys: | + ${{ runner.os }}-ros-${{ matrix.rosdistro }}-apt- + + - name: Setup ROS environment + uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: ${{ matrix.rosdistro }} + - name: Search packages in this repository id: list_packages run: | @@ -67,12 +88,16 @@ jobs: - name: Resolve rosdep and install colcon mixin run: | - apt-get update - apt-get install -y python3-pip python3-colcon-lcov-result lcov unzip gcovr + sudo apt-get update + sudo apt-get install -y python3-pip python3-colcon-lcov-result lcov unzip gcovr libunwind-dev rosdep update --include-eol-distros rosdep install -iy --from-paths src --rosdistro ${{ matrix.rosdistro }} colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml colcon mixin update default + df -h + sudo apt-get clean + sudo rm -rf /var/lib/apt/lists/* + df -h shell: bash - name: Install Build Wrapper diff --git a/common/agnocast_wrapper/CHANGELOG.rst b/common/agnocast_wrapper/CHANGELOG.rst index c1512f20c53..637344eb25a 100644 --- a/common/agnocast_wrapper/CHANGELOG.rst +++ b/common/agnocast_wrapper/CHANGELOG.rst @@ -5,6 +5,14 @@ Changelog for package agnocast_wrapper 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/common/agnocast_wrapper/package.xml b/common/agnocast_wrapper/package.xml index dca9db724c9..30f1de1d363 100644 --- a/common/agnocast_wrapper/package.xml +++ b/common/agnocast_wrapper/package.xml @@ -2,7 +2,7 @@ agnocast_wrapper - 17.5.3 + 17.6.1 A wrapper package for agnocast Mateusz Palczuk Apache License 2.0 diff --git a/common/get_parameter/CHANGELOG.rst b/common/get_parameter/CHANGELOG.rst index 87e600e2929..b9553dc060b 100644 --- a/common/get_parameter/CHANGELOG.rst +++ b/common/get_parameter/CHANGELOG.rst @@ -5,6 +5,14 @@ Changelog for package get_parameter 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/common/get_parameter/package.xml b/common/get_parameter/package.xml index 108b60c49c4..0b1069e33d0 100644 --- a/common/get_parameter/package.xml +++ b/common/get_parameter/package.xml @@ -2,7 +2,7 @@ get_parameter - 17.5.3 + 17.6.1 Thin wrapper for rclcpp::Node::get_parameter Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index a02e5df637f..ed510116478 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package arithmetic 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp index b720e58a09c..d6287f625c3 100644 --- a/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp +++ b/common/math/arithmetic/include/arithmetic/floating_point/comparison.hpp @@ -23,35 +23,27 @@ namespace math namespace arithmetic { template -auto isApproximatelyEqualTo(T a, T b) +constexpr auto isApproximatelyEqualTo(T a, T b) -> bool { return std::abs(a - b) <= (std::numeric_limits::epsilon() * std::max(std::abs(a), std::abs(b))); } template -auto isEssentiallyEqualTo(T a, T b) +constexpr auto isEssentiallyEqualTo(T a, T b) -> bool { return std::abs(a - b) <= (std::numeric_limits::epsilon() * std::min(std::abs(a), std::abs(b))); } -template -auto isDefinitelyLessThan(T a, T b, Ts... xs) +template +constexpr auto isDefinitelyLessThan(T a, T b) -> bool { - auto compare = [](T a, T b) { - return (b - a) > (std::numeric_limits::epsilon() * std::max(std::abs(a), std::abs(b))); - }; - - if constexpr (0 < sizeof...(Ts)) { - return compare(a, b) and compare(b, xs...); - } else { - return compare(a, b); - } + return (b - a) > (std::numeric_limits::epsilon() * std::max(std::abs(a), std::abs(b))); } template -auto isDefinitelyGreaterThan(T a, T b) +constexpr auto isDefinitelyGreaterThan(T a, T b) -> bool { return (a - b) > (std::numeric_limits::epsilon() * std::max(std::abs(a), std::abs(b))); } diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 4699b1c2d31..e0d8acc2bc8 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 17.5.3 + 17.6.1 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 6dfafb2f761..2d94ce013f3 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package geometry 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/common/math/geometry/include/geometry/vector3/is_finite.hpp b/common/math/geometry/include/geometry/vector3/is_finite.hpp new file mode 100644 index 00000000000..d3f8cca8a3f --- /dev/null +++ b/common/math/geometry/include/geometry/vector3/is_finite.hpp @@ -0,0 +1,32 @@ +// 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__VECTOR3__IS_FINITE_HPP_ +#define GEOMETRY__VECTOR3__IS_FINITE_HPP_ + +#include + +namespace math +{ +namespace geometry +{ +template ::value, std::nullptr_t> = nullptr> +auto isFinite(const T & v) -> bool +{ + return std::isfinite(v.x) and std::isfinite(v.y) and std::isfinite(v.z); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__VECTOR3__IS_FINITE_HPP_ diff --git a/common/math/geometry/include/geometry/vector3/rotate.hpp b/common/math/geometry/include/geometry/vector3/rotate.hpp new file mode 100644 index 00000000000..19cd492ddeb --- /dev/null +++ b/common/math/geometry/include/geometry/vector3/rotate.hpp @@ -0,0 +1,44 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GEOMETRY__VECTOR3__ROTATE_HPP_ +#define GEOMETRY__VECTOR3__ROTATE_HPP_ + +#include +#include +#include + +namespace math +{ +namespace geometry +{ +template < + typename V, typename Q, + std::enable_if_t, IsLikeQuaternion>, std::nullptr_t> = + nullptr> +auto rotate(const V & vector, const Q & quaternion) +{ + const Eigen::Quaterniond eigen_quaternion(quaternion.w, quaternion.x, quaternion.y, quaternion.z); + const Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z); + const Eigen::Vector3d eigen_rotated_vector = eigen_quaternion * eigen_vector; + V rotated_vector; + rotated_vector.x = eigen_rotated_vector.x(); + rotated_vector.y = eigen_rotated_vector.y(); + rotated_vector.z = eigen_rotated_vector.z(); + return rotated_vector; +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__VECTOR3__ROTATE_HPP_ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index c9892c8a072..4fc9235157f 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 17.5.3 + 17.6.1 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 8b9af983b44..c1e7449883f 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package scenario_simulator_exception 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index f5c79e63f66..b451e599b60 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 17.5.3 + 17.6.1 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 f6ea59c80bf..209f1354dc0 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package junit_exporter 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 1ca4cd51700..f20c44381c1 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 17.5.3 + 17.6.1 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 c39c48b8b77..3b95ca4e32a 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package status_monitor 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index dbd6c37ff13..817d26232b1 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 17.5.3 + 17.6.1 none Tatsuya Yamasaki Apache License 2.0 diff --git a/docs/developer_guide/Parameters.md b/docs/developer_guide/Parameters.md index 3c84676da27..616bba5fba2 100644 --- a/docs/developer_guide/Parameters.md +++ b/docs/developer_guide/Parameters.md @@ -473,18 +473,34 @@ which differs from the elliptical coordinate system used in v2/v3. ![noise v4 coordinate system](./images/parameters/noise_v4.png) -### `noise.v4..position..mean` +### `noise.v4..ellipse_y_radii` -A `double` type value, default `0.0`. -The mean value for the X/Y-axis position noise distribution. -The noise models the time series as AR(1) model. +Array of positive double type values, default `[10.0, 20.0, 40.0, 60.0, 80.0, 120.0, 150.0, 180.0, 1000.0]`. Units are in meters. +See [`ellipse_y_radii` documentation of noise v2](#noisev2ellipse_y_radii) for detailed explanation. This parameter is used only if the value of `noise.model.version` is `4`. -### `noise.v4..position..standard_deviation` +### `noise.v4..position..mean.ellipse_normalized_x_radius` -A positive `double` type value, default `0.0`. -The standard deviation for the X/Y-axis position noise distribution. -The noise models the time series as AR(1) model. +A positive `double` type value, default `1.0`. +See [`ellipse_normalized_x_radius` documentation of noise v2](#noisev2distancemeanellipse_normalized_x_radius) for detailed explanation. +This parameter is used only if the value of `noise.model.version` is `4`. + +### `noise.v4..position..mean.values` + +Array of double type values, default `[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]`. +See [`values` documentation of noise v2](#noisev2distancemeanvalues) for detailed explanation. +This parameter is used only if the value of `noise.model.version` is `4`. + +### `noise.v4..position..standard_deviation.ellipse_normalized_x_radius` + +A positive `double` type value, default `1.0`. +See [`ellipse_normalized_x_radius` documentation of noise v2](#noisev2distancestandarddeviationellipse_normalized_x_radius) for detailed explanation. +This parameter is used only if the value of `noise.model.version` is `4`. + +### `noise.v4..position..standard_deviation.values` + +Array of positive double type values, default `[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]`. +See [`values` documentation of noise v2](#noisev2distancestandarddeviationvalues) for detailed explanation. This parameter is used only if the value of `noise.model.version` is `4`. ### `noise.v4..position..autocorrelation_coefficient.amplitude` @@ -505,60 +521,80 @@ A positive `double` type value, default `0.0`. The parameter of the autocorrelation coefficient used in the generation of X/Y-axis position noise. This parameter is used only if the value of `noise.model.version` is `4`. -### `noise.v4..rotation` (namespace) +### `noise.v4..yaw` (namespace) -The rotation noise in noise v4 model applies angular noise around the closest point on the entity's bounding box +The rotation yaw noise in noise v4 model applies angular noise around the closest point on the entity's bounding box to the ego vehicle (the origin of noise coordinate), rather than around the entity's base-link. -### `noise.v4..rotation.mean` +### `noise.v4..yaw.mean.ellipse_normalized_x_radius` -A `double` type value, default `0.0`. -The mean value for the rotation noise distribution. -The unit is radians. The noise models the time series as AR(1) model. +A positive `double` type value, default `1.0`. +See [`ellipse_normalized_x_radius` documentation of noise v2](#noisev2yawmeanellipse_normalized_x_radius) for detailed explanation. This parameter is used only if the value of `noise.model.version` is `4`. -### `noise.v4..rotation.standard_deviation` +### `noise.v4..yaw.mean.values` -A positive `double` type value, default `0.0`. -The standard deviation for the rotation noise distribution. The unit is radians. -The noise models the time series as AR(1) model. This parameter is used only if the value of `noise.model.version` is `4`. +Array of double type values, default `[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]`. +See [`values` documentation of noise v2](#noisev2yawmeanvalues) for detailed explanation. +The unit is radians. This parameter is used only if the value of `noise.model.version` is `4`. + +### `noise.v4..yaw.standard_deviation.ellipse_normalized_x_radius` + +A positive `double` type value, default `1.0`. +See [`ellipse_normalized_x_radius` documentation of noise v2](#noisev2yawstandarddeviationellipse_normalized_x_radius) for detailed explanation. +This parameter is used only if the value of `noise.model.version` is `4`. + +### `noise.v4..yaw.standard_deviation.values` -### `noise.v4..rotation.autocorrelation_coefficient.amplitude` +Array of positive double type values, default `[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]`. +See [`values` documentation of noise v2](#noisev2yawstandarddeviationvalues) for detailed explanation. +The unit is radians. This parameter is used only if the value of `noise.model.version` is `4`. + +### `noise.v4..yaw.autocorrelation_coefficient.amplitude` A positive `double` type value, default `0.0`. The parameter of the autocorrelation coefficient used in the generation of rotation noise. This parameter is used only if the value of `noise.model.version` is `4`. -### `noise.v4..rotation.autocorrelation_coefficient.decay` +### `noise.v4..yaw.autocorrelation_coefficient.decay` A positive `double` type value, default `0.0`. The parameter of the autocorrelation coefficient used in the generation of rotation noise. This parameter is used only if the value of `noise.model.version` is `4`. -### `noise.v4..rotation.autocorrelation_coefficient.offset` +### `noise.v4..yaw.autocorrelation_coefficient.offset` A positive `double` type value, default `0.0`. The parameter of the autocorrelation coefficient used in the generation of rotation noise. This parameter is used only if the value of `noise.model.version` is `4`. +### `noise.v4..yaw_flip` (namespace) + +The yaw-flip noise in noise v4 model is basically same to one in noise v2. +See [`yaw_flip` documentation of noise v2](#noisev2yaw_flipautocorrelation_coefficientamplitude) for detailed explanation. +Like noise v2, noise v4 also has the parameters below in this namespace. + +- `noise.v4..yaw_flip.autocorrelation_coefficient.amplitude` +- `noise.v4..yaw_flip.autocorrelation_coefficient.decay` +- `noise.v4..yaw_flip.autocorrelation_coefficient.offset` +- `noise.v4..yaw_flip.rate` +- `noise.v4..yaw_flip.speed_threshold` + ### `noise.v4..true_positive` (namespace) -The true positive parameters in noise v4 model control the detection probability of entities based on distance from the ego vehicle. -Unlike v2's elliptical coordinate system, v4 uses direct distance measurement to the closest point on the entity's bounding box. +The true positive parameters in noise v4 model control the detection probability of entities based on distance from the ego vehicle. +Like v2/v3, v4 uses elliptical coordinate system to determine the detection rate based on the distance from the ego vehicle. -### `noise.v4..true_positive.rate.distance_thresholds` +### `noise.v4..true_positive.rate.ellipse_normalized_x_radius` -Array of positive double type values, default `[1000.0]`. Units are in meters. -Distance thresholds used to determine which detection rate to apply. -The distance is calculated from the ego vehicle base-link to the closest point on the entity's bounding box. +A positive `double` type value, default `1.0`. +See [`ellipse_normalized_x_radius` documentation of noise v2](#noisev2true_positiverateellipse_normalized_x_radius) for detailed explanation. This parameter is used only if the value of `noise.model.version` is `4`. ### `noise.v4..true_positive.rate.values` -Array of double type values between `0.0` and `1.0`, default `[1.0]`. -Each element represents the detection probability for the corresponding distance range. -The array size must match `distance_thresholds`. -The first threshold greater than the calculated distance determines which rate value to use. +Array of double type values between `0.0` and `1.0`, default `[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]`. +See [`values` documentation of noise v2](#noisev2true_positiveratevalues) for detailed explanation. This parameter is used only if the value of `noise.model.version` is `4`. ### `noise.v4..true_positive.autocorrelation_coefficient.amplitude` diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index c9b288c6ba1..b71b5ad44c8 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package concealer 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 30f1e4dbc93..e11eaa53c54 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 17.5.3 + 17.6.1 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index edca78029fc..1e487ea7737 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -27,6 +27,14 @@ Changelog for package embree_vendor 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 18c345485be..23bb91e5ff3 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 17.5.3 + 17.6.1 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/external/zmqpp_vendor/CHANGELOG.rst b/external/zmqpp_vendor/CHANGELOG.rst index fb008a4c6a5..7fd863a1bae 100644 --- a/external/zmqpp_vendor/CHANGELOG.rst +++ b/external/zmqpp_vendor/CHANGELOG.rst @@ -18,6 +18,14 @@ Changelog for package zmqpp_vendor 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/external/zmqpp_vendor/package.xml b/external/zmqpp_vendor/package.xml index 6573ea5e5a7..f97ff97379b 100644 --- a/external/zmqpp_vendor/package.xml +++ b/external/zmqpp_vendor/package.xml @@ -2,7 +2,7 @@ zmqpp_vendor - 17.5.3 + 17.6.1 Vendor package for zmqpp Masaya Kataoka Apache License 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index 4c4da4aebe9..74acad9174a 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package kashiwanoha_map 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 4de472b1b04..d4140e39b19 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 17.5.3 + 17.6.1 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst index 1b2a5d829e0..6eadc6aedcf 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -12,6 +12,14 @@ Changelog for package simple_cross_map 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index a8746c50bb5..9b78c36cced 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 17.5.3 + 17.6.1 map package for simple cross Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index d30c1ec6a4f..1b6184ecb36 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package cpp_mock_scenarios 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index c74976353a1..60f00064da0 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 17.5.3 + 17.6.1 C++ mock scenarios masaya Apache License 2.0 diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index b43896b4103..8e3eb28c709 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_experimental_catalog 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index 4123dfecebd..397fa9c1db7 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 17.5.3 + 17.6.1 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 4bc8c0a3d08..6ef2b155a08 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -35,6 +35,14 @@ Changelog for package openscenario_interpreter 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp index d821cb079d5..f998aa9d8c3 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/follow_trajectory_action.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include namespace openscenario_interpreter { diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 97d213c4087..f4e2b91582c 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 17.5.3 + 17.6.1 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index ba3c58f471f..cd631193d7c 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_interpreter_example 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index b0be1f88847..e711809a341 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 17.5.3 + 17.6.1 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 bca530c5658..ed78ab8dec5 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_interpreter_msgs 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index bd3e1fa66c1..ae43d89e5e9 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 17.5.3 + 17.6.1 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 9b4bdee5c89..2b6c8970e4a 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_preprocessor 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 6f738789fd7..e9a8404741b 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 17.5.3 + 17.6.1 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 a3c6de5235a..536ab2ab2b5 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_preprocessor_msgs 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index 75a703e02d2..cccc00f0f55 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 17.5.3 + 17.6.1 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 14d9301886d..b64bddd6ddc 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -27,6 +27,14 @@ Changelog for package openscenario_utility 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge pull request `#1683 `_ from tier4/fix/macro-xml-validation diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index e26afbc64a2..bb701d45453 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 17.5.3 + 17.6.1 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index 70aa3d2a178..e19c62e26bc 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -13,6 +13,14 @@ Changelog for package openscenario_validator 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 908229cd27b..643f5f1727d 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 17.5.3 + 17.6.1 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index 30077046e95..9b4eac83c3d 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_visualization 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 173dd29656e..ede68c58732 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 17.5.3 + 17.6.1 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index 0515ff222a1..2bf694bf7e9 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package real_time_factor_control_rviz_plugin 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml index f14856e2658..f2d0d236157 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/package.xml @@ -2,7 +2,7 @@ real_time_factor_control_rviz_plugin - 17.5.3 + 17.6.1 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 7fb7c04d6ee..9b2a37d4ffa 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package scenario_simulator_v2 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 498a0dd2350..182f417c0c0 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 17.5.3 + 17.6.1 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 d8765315608..bef6443800f 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package behavior_tree_plugin 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 9c2043d39b1..12d1befb0e8 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 17.5.3 + 17.6.1 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 93c22986ce1..a625b3dba89 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 @@ -73,19 +73,18 @@ bool FollowPolylineTrajectoryAction::checkPreconditions() auto FollowPolylineTrajectoryAction::doAction() -> BT::NodeStatus { - auto getTargetSpeed = [&]() -> double { - if (target_speed_.has_value()) { - return target_speed_.value(); - } else { - return canonicalized_entity_status_->getTwist().linear.x; - } + const auto getTargetSpeed = [this]() -> double { + return target_speed_.value_or(canonicalized_entity_status_->getTwist().linear.x); }; - if ( - const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*canonicalized_entity_status_), - *polyline_trajectory, behavior_parameter_, step_time_, - default_matching_distance_for_lanelet_pose_calculation_, getTargetSpeed())) { + if (const auto entity_status_updated = + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*canonicalized_entity_status_), + behavior_parameter_, step_time_), + *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation_, + getTargetSpeed(), step_time_); + entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); 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 3bfeb041029..b0b33124b06 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 @@ -71,21 +71,20 @@ bool FollowPolylineTrajectoryAction::checkPreconditions() } } -BT::NodeStatus FollowPolylineTrajectoryAction::doAction() +auto FollowPolylineTrajectoryAction::doAction() -> BT::NodeStatus { - auto getTargetSpeed = [&]() -> double { - if (target_speed_.has_value()) { - return target_speed_.value(); - } else { - return canonicalized_entity_status_->getTwist().linear.x; - } + const auto getTargetSpeed = [this]() -> double { + return target_speed_.value_or(canonicalized_entity_status_->getTwist().linear.x); }; - if ( - const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*canonicalized_entity_status_), - *polyline_trajectory, behavior_parameter_, step_time_, - default_matching_distance_for_lanelet_pose_calculation_, getTargetSpeed())) { + if (const auto entity_status_updated = + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*canonicalized_entity_status_), + behavior_parameter_, step_time_), + *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation_, + getTargetSpeed(), step_time_); + entity_status_updated.has_value()) { setCanonicalizedEntityStatus(entity_status_updated.value()); setOutput("waypoints", calculateWaypoints()); setOutput("obstacle", calculateObstacle(calculateWaypoints())); diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index eed0f7bedd5..001a40e47cc 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package do_nothing_plugin 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 92cea17c4ab..9e1c660ac6a 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 17.5.3 + 17.6.1 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 604080c3ff1..10a903674c7 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -24,6 +24,25 @@ Changelog for package simple_sensor_simulator 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge pull request `#1685 `_ from tier4/fix/noise_v4 +* Merge branch 'master' into fix/noise_v4 +* use elliptical model for x,y,yaw in noise_v4 +* Apply noise v4 correctly +* Implement yaw flip noise correctly +* Add missing parts for noise_v4 +* rename rotation to yaw in noise_v4 +* use same true positive calculation algorithm to noise_v2 in noise_v4 +* commonize true positive calculation algorithm in noise_v2 +* Add create_selector lambda function to make selector lambda more common +* rename noise.v4..rotation into noise.v4..yaw +* Implement yaw_flip noise for noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp index ddcbbfd0336..8548222ac0e 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/detection_sensor/detection_sensor.hpp @@ -94,7 +94,7 @@ class DetectionSensor : public DetectionSensorBase bool true_positive, flip; // for noise v4 - double v4_position_x_noise, v4_position_y_noise, v4_rotation_noise; + double v4_position_x_noise, v4_position_y_noise; std::string config_name; @@ -106,7 +106,6 @@ class DetectionSensor : public DetectionSensorBase flip(false), v4_position_x_noise(0.0), v4_position_y_noise(0.0), - v4_rotation_noise(0.0), config_name("") { } diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 4da7c4d4bb0..907834c8e4c 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -2,7 +2,7 @@ simple_sensor_simulator - 17.5.3 + 17.6.1 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index d3f4e9ccae6..629fd716519 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -393,6 +394,93 @@ auto DetectionSensor::update( return std::clamp(amplitude * std::exp(-decay * interval) + offset, 0.0, 1.0); }; + auto create_selector = [](const std::string & parameter_base_path, double x, double y) { + return [parameter_base_path, x, y](const std::string & name) { + return [=]() { + const auto ellipse_y_radii = + common::getParameter>(parameter_base_path + "ellipse_y_radii"); + const auto ellipse_normalized_x_radius = common::getParameter( + parameter_base_path + name + ".ellipse_normalized_x_radius"); + const auto values = + common::getParameter>(parameter_base_path + name + ".values"); + if (ellipse_y_radii.size() == values.size()) { + /* + If the parameter `ellipse_y_radii` contains the value 0.0, + division by zero will occur here. + However, in that case, the distance will be NaN, which correctly + expresses the meaning that "the distance cannot be defined", and + this function will work without any problems (zero will be + returned). + */ + const auto distance = std::hypot(x / ellipse_normalized_x_radius, y); + for (auto i = std::size_t(0); i < ellipse_y_radii.size(); ++i) { + if (distance < ellipse_y_radii[i]) { + return values[i]; + } + } + return 0.0; + } else { + throw common::Error( + "Array size mismatch: ", std::quoted(parameter_base_path + "ellipse_y_radii"), + " has ", ellipse_y_radii.size(), " elements, but ", + std::quoted(parameter_base_path + name + ".values"), " has ", values.size(), + " elements. Both arrays must have the same size."); + } + }; + }; + }; + + auto yaw_flip = [&]( + bool previous_flip, double speed, double interval, + const std::string & parameter_base_path) -> bool { + const auto speed_threshold = + common::getParameter(parameter_base_path + "yaw_flip.speed_threshold"); + const auto rate = common::getParameter(parameter_base_path + "yaw_flip.rate"); + return speed < speed_threshold and + markov_process_noise( + previous_flip, rate, + autocorrelation_coefficient(parameter_base_path + "yaw_flip", interval)); + }; + + auto true_positive = [&]( + bool previous_true_positive, double interval, + const std::string & parameter_base_path, double rate) -> bool { + return markov_process_noise( + previous_true_positive, rate, + autocorrelation_coefficient(parameter_base_path + "true_positive", interval)); + }; + + auto apply_yaw_flip = [&](traffic_simulator_msgs::EntityStatus & entity) -> void { + geometry_msgs::msg::Point local_center_point; + simulation_interface::toMsg(entity.bounding_box().center(), local_center_point); + + geometry_msgs::msg::Pose original_pose; + simulation_interface::toMsg(entity.pose(), original_pose); + + tf2::Quaternion original_orientation; + tf2::fromMsg(original_pose.orientation, original_orientation); + const tf2::Quaternion flip_rotation( + tf2::Vector3(0, 0, 1), boost::math::constants::pi()); + + geometry_msgs::msg::Pose flipped_center_pose; + flipped_center_pose.position = + math::geometry::transformPoint(original_pose, local_center_point); + flipped_center_pose.orientation = tf2::toMsg(original_orientation * flip_rotation); + + geometry_msgs::msg::Point center_offset_negative; + center_offset_negative.x = -entity.bounding_box().center().x(); + center_offset_negative.y = -entity.bounding_box().center().y(); + center_offset_negative.z = -entity.bounding_box().center().z(); + + const auto new_base_link_position = + math::geometry::transformPoint(flipped_center_pose, center_offset_negative); + + simulation_interface::toProto( + new_base_link_position, *entity.mutable_pose()->mutable_position()); + simulation_interface::toProto( + flipped_center_pose.orientation, *entity.mutable_pose()->mutable_orientation()); + }; + auto noise_v2 = [&]( const auto & detected_entities, auto simulation_time, const std::string & version_namespace) { @@ -416,45 +504,7 @@ auto DetectionSensor::update( std::string(detected_objects_publisher->get_topic_name()) + ".noise." + version_namespace + "."; - auto parameter = [this, version_base_path](const auto & name) { - return common::getParameter(version_base_path + name); - }; - - auto parameters = [this, version_base_path](const auto & name) { - return common::getParameter>(version_base_path + name); - }; - - auto selector = [&](const std::string & name) { - return [ellipse_y_radii = parameters("ellipse_y_radii"), - ellipse_normalized_x_radius = parameter(name + ".ellipse_normalized_x_radius"), - values = parameters(name + ".values"), &x, &y, version_namespace, name]() { - if (ellipse_y_radii.size() == values.size()) { - /* - If the parameter `.noise.v2.ellipse_y_radii` - contains the value 0.0, division by zero will occur here. - However, in that case, the distance will be NaN, which correctly - expresses the meaning that "the distance cannot be defined", and - this function will work without any problems (zero will be - returned). - */ - const auto distance = std::hypot(x / ellipse_normalized_x_radius, y); - for (auto i = std::size_t(0); i < ellipse_y_radii.size(); ++i) { - if (distance < ellipse_y_radii[i]) { - return values[i]; - } - } - return 0.0; - } else { - throw common::Error( - "Array size mismatch: ", std::quoted("ellipse_y_radii"), " has ", - ellipse_y_radii.size(), " elements, but ", std::quoted(name + ".values"), " has ", - values.size(), - " elements. Both arrays must have the same size in namespace for noise model " - "version ", - std::quoted(version_namespace), "."); - } - }; - }; + auto selector = create_selector(version_base_path, x, y); noise_output->second.distance_noise = [&]() { const auto mean = selector("distance.mean"); @@ -472,21 +522,12 @@ auto DetectionSensor::update( autocorrelation_coefficient(version_base_path + "yaw", interval)); }(); - noise_output->second.flip = [&]() { - const auto speed_threshold = parameter("yaw_flip.speed_threshold"); - const auto rate = parameter("yaw_flip.rate"); - return speed < speed_threshold and - markov_process_noise( - noise_output->second.flip, rate, - autocorrelation_coefficient(version_base_path + "yaw_flip", interval)); - }(); + noise_output->second.flip = + yaw_flip(noise_output->second.flip, speed, interval, version_base_path); - noise_output->second.true_positive = [&]() { - const auto rate = selector("true_positive.rate"); - return markov_process_noise( - noise_output->second.true_positive, rate(), - autocorrelation_coefficient(version_base_path + "true_positive", interval)); - }(); + noise_output->second.true_positive = true_positive( + noise_output->second.true_positive, interval, version_base_path, + selector("true_positive.rate")()); if (noise_output->second.true_positive) { const auto angle = std::atan2(y, x); @@ -632,32 +673,9 @@ auto DetectionSensor::update( const auto interval = simulation_time - std::exchange(noise_output->second.simulation_time, simulation_time); - noise_output->second.v4_position_x_noise = [&]() { - const auto mean = common::getParameter(parameter_base_path + "position.x.mean"); - const auto standard_deviation = - common::getParameter(parameter_base_path + "position.x.standard_deviation"); - return autoregressive_noise( - noise_output->second.v4_position_x_noise, mean, standard_deviation, - autocorrelation_coefficient(parameter_base_path + "position.x", interval)); - }(); - - noise_output->second.v4_position_y_noise = [&]() { - const auto mean = common::getParameter(parameter_base_path + "position.y.mean"); - const auto standard_deviation = - common::getParameter(parameter_base_path + "position.y.standard_deviation"); - return autoregressive_noise( - noise_output->second.v4_position_y_noise, mean, standard_deviation, - autocorrelation_coefficient(parameter_base_path + "position.y", interval)); - }(); - - noise_output->second.v4_rotation_noise = [&]() { - const auto mean = common::getParameter(parameter_base_path + "rotation.mean"); - const auto standard_deviation = - common::getParameter(parameter_base_path + "rotation.standard_deviation"); - return autoregressive_noise( - noise_output->second.v4_rotation_noise, mean, standard_deviation, - autocorrelation_coefficient(parameter_base_path + "rotation", interval)); - }(); + const auto speed = std::hypot( + vanilla_entity.action_status().twist().linear().x(), + vanilla_entity.action_status().twist().linear().y()); math::geometry::boost_point ego_baselink_2d( ego_entity_status->pose().position().x(), ego_entity_status->pose().position().y()); @@ -672,36 +690,41 @@ auto DetectionSensor::update( ego_baselink_2d, math::geometry::toPolygon2D(entity_pose, entity_bounding_box)); }(); - noise_output->second.true_positive = [&]() { - const auto distance_thresholds = common::getParameter>( - parameter_base_path + "true_positive.rate.distance_thresholds"); - const auto rate_values = common::getParameter>( - parameter_base_path + "true_positive.rate.values"); + auto selector = create_selector( + parameter_base_path, noise_base.x() - ego_baselink_2d.x(), + noise_base.y() - ego_baselink_2d.y()); - if (distance_thresholds.size() != rate_values.size()) { - throw common::Error( - "Array size mismatch: ", parameter_base_path, - "true_positive.distance_thresholds has ", distance_thresholds.size(), - " elements, but ", parameter_base_path, "true_positive.rate.values has ", - rate_values.size(), " elements. Both arrays must have the same size."); - } else { - // Find the first threshold greater than the calculated distance - const auto rate = [&, distance_to_noise_base = std::hypot( - noise_base.x() - ego_baselink_2d.x(), - noise_base.y() - ego_baselink_2d.y())]() -> double { - for (size_t i = 0; i < distance_thresholds.size(); ++i) { - if (distance_to_noise_base < distance_thresholds[i]) { - return rate_values[i]; - } - } - return 0.0; // If distance exceeds all thresholds - }(); + noise_output->second.v4_position_x_noise = [&]() { + const auto mean = selector("position.x.mean"); + const auto standard_deviation = selector("position.x.standard_deviation"); + return autoregressive_noise( + noise_output->second.v4_position_x_noise, mean(), standard_deviation(), + autocorrelation_coefficient(parameter_base_path + "position.x", interval)); + }(); - return markov_process_noise( - noise_output->second.true_positive, rate, - autocorrelation_coefficient(parameter_base_path + "true_positive", interval)); - } + noise_output->second.v4_position_y_noise = [&]() { + const auto mean = selector("position.y.mean"); + const auto standard_deviation = selector("position.y.standard_deviation"); + return autoregressive_noise( + noise_output->second.v4_position_y_noise, mean(), standard_deviation(), + autocorrelation_coefficient(parameter_base_path + "position.y", interval)); + }(); + + noise_output->second.yaw_noise = [&]() { + const auto mean = selector("yaw.mean"); + const auto standard_deviation = selector("yaw.standard_deviation"); + return autoregressive_noise( + noise_output->second.yaw_noise, mean(), standard_deviation(), + autocorrelation_coefficient(parameter_base_path + "yaw", interval)); }(); + + noise_output->second.flip = + yaw_flip(noise_output->second.flip, speed, interval, parameter_base_path); + + noise_output->second.true_positive = true_positive( + noise_output->second.true_positive, interval, parameter_base_path, + selector("true_positive.rate")()); + if (not noise_output->second.true_positive) { return std::nullopt; } else { @@ -721,39 +744,38 @@ auto DetectionSensor::update( // If ego and entity are too close, skip the noise application return vanilla_entity; } else { - const auto [final_pos, final_orientation] = - [&]() -> std::tuple { - tf2::Quaternion rotation_noise( - tf2::Vector3(0, 0, 1), noise_output->second.v4_rotation_noise); - - // Apply rotation to entity position around noise_base - tf2::Vector3 entity_pos( - vanilla_entity.pose().position().x(), vanilla_entity.pose().position().y(), 0.0); - tf2::Vector3 noise_base_pos(noise_base.x(), noise_base.y(), 0.0); - tf2::Vector3 offset = entity_pos - noise_base_pos; - tf2::Vector3 rotated_offset = tf2::quatRotate(rotation_noise, offset); - tf2::Vector3 rotated_pos = noise_base_pos + rotated_offset; - - // Create local coordinate system and apply position noise - tf2::Vector3 y_unit(y_unit_x, y_unit_y, 0.0); - tf2::Vector3 x_unit(y_unit_y, -y_unit_x, 0.0); - tf2::Vector3 noise_world = noise_output->second.v4_position_x_noise * x_unit + - noise_output->second.v4_position_y_noise * y_unit; - - tf2::Quaternion original_orientation( - vanilla_entity.pose().orientation().x(), vanilla_entity.pose().orientation().y(), - vanilla_entity.pose().orientation().z(), vanilla_entity.pose().orientation().w()); - - return {rotated_pos + noise_world, original_orientation * rotation_noise}; - }(); + tf2::Quaternion yaw_rotation_noise( + tf2::Vector3(0, 0, 1), noise_output->second.yaw_noise); + + geometry_msgs::msg::Pose original_pose; + simulation_interface::toMsg(vanilla_entity.pose(), original_pose); + + tf2::Vector3 entity_position; + tf2::fromMsg(original_pose.position, entity_position); + tf2::Vector3 noise_base_pos(noise_base.x(), noise_base.y(), 0.0); + tf2::Vector3 offset = entity_position - noise_base_pos; + const auto position_after_yaw_noise = + noise_base_pos + tf2::quatRotate(yaw_rotation_noise, offset); + + tf2::Vector3 y_unit(y_unit_x, y_unit_y, 0.0); + tf2::Vector3 x_unit(y_unit_y, -y_unit_x, 0.0); + const auto noise_world = noise_output->second.v4_position_x_noise * x_unit + + noise_output->second.v4_position_y_noise * y_unit; + + const auto final_position = position_after_yaw_noise + noise_world; + tf2::Quaternion original_orientation; + tf2::fromMsg(original_pose.orientation, original_orientation); + const auto final_orientation = original_orientation * yaw_rotation_noise; auto noised_entity = vanilla_entity; - noised_entity.mutable_pose()->mutable_position()->set_x(final_pos.x()); - noised_entity.mutable_pose()->mutable_position()->set_y(final_pos.y()); - noised_entity.mutable_pose()->mutable_orientation()->set_x(final_orientation.getX()); - noised_entity.mutable_pose()->mutable_orientation()->set_y(final_orientation.getY()); - noised_entity.mutable_pose()->mutable_orientation()->set_z(final_orientation.getZ()); - noised_entity.mutable_pose()->mutable_orientation()->set_w(final_orientation.getW()); + noised_entity.mutable_pose()->mutable_position()->set_x(final_position.x()); + noised_entity.mutable_pose()->mutable_position()->set_y(final_position.y()); + simulation_interface::toProto( + tf2::toMsg(final_orientation), *noised_entity.mutable_pose()->mutable_orientation()); + + if (noise_output->second.flip) { + apply_yaw_flip(noised_entity); + } return noised_entity; } diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index c98af37d3e8..ae5b81aab77 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package simulation_interface 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 3965cdea05b..b7f4d994326 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 17.5.3 + 17.6.1 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index bd107e36941..83994bbf165 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package traffic_simulator 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 85ff6118c72..1aa38f607ad 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -28,8 +28,10 @@ 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/follow_trajectory/follow_waypoint_controller.cpp + src/behavior/follow_trajectory/follow_trajectory.cpp + src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp + src/behavior/follow_trajectory/validated_entity_status.cpp src/behavior/longitudinal_speed_planning.cpp src/behavior/route_planner.cpp src/color_utils/color_utils.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp index 15491cd4119..57dbbb25930 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp similarity index 54% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp index cb6ecf01a51..be724d4d4de 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_trajectory.hpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ -#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ #include +#include +#include +#include #include #include -#include #include #include @@ -26,12 +28,14 @@ namespace traffic_simulator { namespace follow_trajectory { -auto makeUpdatedStatus( - const traffic_simulator_msgs::msg::EntityStatus &, - traffic_simulator_msgs::msg::PolylineTrajectory &, - const traffic_simulator_msgs::msg::BehaviorParameter &, double, double, - std::optional target_speed = std::nullopt) -> std::optional; +/// @note side effects on polyline_trajectory +auto makeUpdatedEntityStatus( + const ValidatedEntityStatus & validated_entity_status, + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const std::optional target_speed, const double step_time) + -> std::optional; + } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_TRAJECTORY_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp similarity index 92% rename from simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp rename to simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp index 6df1320c203..27197eeb1ed 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_waypoint_controller.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/follow_waypoint_controller.hpp @@ -12,8 +12,8 @@ // 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_ +#ifndef TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_ #include #include @@ -30,11 +30,11 @@ namespace traffic_simulator { namespace follow_trajectory { -struct ControllerError : public common::Error +struct ControllerError : public common::SimulationError { template explicit ControllerError(Ts &&... xs) - : common::Error(common::concatenate( + : common::SimulationError(common::concatenate( "An error occurred in the internal controller operation in the FollowTrajectoryAction. ", "Please report the following information to the developer: ", std::forward(xs)...)) @@ -59,7 +59,7 @@ struct PredictedState auto isImmobile(const double tolerance) const { - return std::abs(speed) < tolerance && std::abs(acceleration) < tolerance; + return std::abs(speed) < tolerance and std::abs(acceleration) < tolerance; } template @@ -129,7 +129,7 @@ class FollowWaypointController 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 = 30; + static constexpr std::size_t number_of_acceleration_candidates = 30UL; /* This is a debugging method, it is not worth giving it much attention. @@ -231,7 +231,7 @@ class FollowWaypointController 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} + target_speed{target_speed.value_or(max_speed)} { } @@ -242,14 +242,14 @@ class FollowWaypointController const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory) const -> std::string { - if (const auto & vertices = polyline_trajectory.shape.vertices; !vertices.empty()) { + if (const auto & vertices = polyline_trajectory.shape.vertices; not 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)) { + vertices.cbegin(), vertices.cend(), + [](const auto & vertex) { return not std::isnan(vertex.time); }); + first_waypoint_with_arrival_time_specified != vertices.cend()) { 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 " @@ -288,13 +288,13 @@ class FollowWaypointController const double speed) const -> double; auto areConditionsOfArrivalMet( - const double acceleration, const double speed, const double distance) const -> double + const double acceleration, const double speed, const double distance) const -> bool { - return (!with_breaking || std::abs(speed) < local_epsilon) && - std::abs(acceleration) < local_epsilon && distance < finish_distance_tolerance; + return (not with_breaking or std::abs(speed) < local_epsilon) and + std::abs(acceleration) < local_epsilon and distance < finish_distance_tolerance; } }; } // namespace follow_trajectory } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_WAYPOINT_CONTROLLER_HPP_ +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__FOLLOW_WAYPOINT_CONTROLLER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp new file mode 100644 index 00000000000..31beee846cb --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/polyline_trajectory_positioner.hpp @@ -0,0 +1,105 @@ +// 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__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ +using Waypoint = traffic_simulator_msgs::msg::Vertex; +using Waypoints = std::vector; +using WaypointIterator = Waypoints::const_iterator; +struct PolylineTrajectoryPositioner +{ +public: + explicit PolylineTrajectoryPositioner( + const ValidatedEntityStatus & validated_entity_status, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const std::optional target_speed, const double matching_distance, + const double step_time); + + auto makeUpdatedEntityStatus() const -> std::optional; + +private: + // getters + auto getWaypoints() const noexcept(true) -> const Waypoints &; + + auto getNearestWaypointWithSpecifiedTimeIterator() const -> WaypointIterator; + + auto getTimeToWaypoint(const Waypoint & waypoint) const -> double; + + // checks + auto areConditionsOfArrivalMet() const noexcept(true) -> bool; + + auto isAbsoluteBaseTime() const noexcept(true) -> bool; + + auto isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const -> bool; + + auto isNearestWaypointReachable(const double desired_local_acceleration) const -> bool; + + // helpers to the constructor + auto timeToNearestWaypoint() const noexcept(false) -> double; + + auto validatedDistanceToNearestWaypoint() const noexcept(false) -> double; + + auto validatedTotalRemainingDistance() const noexcept(false) -> double; + + auto validatedTotalRemainingTime() const noexcept(false) -> double; + + auto validatedEntityTargetPose() const noexcept(false) -> geometry_msgs::msg::Pose; + + // other validators + auto validatedEntityDesiredLinearAcceleration() const noexcept(false) -> double; + + auto validatedEntityDesiredSpeed(const double desired_local_acceleration) const noexcept(false) + -> double; + + auto validatedEntityDesiredLocalVelocity(const double desired_speed) const noexcept(false) + -> geometry_msgs::msg::Vector3; + + auto validatePredictedState(const double desired_local_acceleration) const noexcept(false) + -> void; + + /// @note 0.0 has been adopted as the start of the simulation + constexpr static double ABSOLUTE_BASE_TIME = 0.0; + + const std::shared_ptr hdmap_utils_ptr_; + const ValidatedEntityStatus validated_entity_status_; + const traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory_; + const double step_time_; + const double matching_distance_; + + const WaypointIterator nearest_waypoint_with_specified_time_it_; + const geometry_msgs::msg::Pose nearest_waypoint_pose_; + const double distance_to_nearest_waypoint_; + const double total_remaining_distance_; + const double time_to_nearest_waypoint_; + const double total_remaining_time_; + + const FollowWaypointController follow_waypoint_controller_; +}; +} // namespace follow_trajectory +} // namespace traffic_simulator + +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__POLYLINE_TRAJECTORY_POSITIONER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.hpp new file mode 100644 index 00000000000..c04d3943da1 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory/validated_entity_status.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 TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__VALIDATED_ENTITY_STATUS_HPP_ +#define TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__VALIDATED_ENTITY_STATUS_HPP_ + +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ +struct ValidatedEntityStatus +{ +public: + explicit ValidatedEntityStatus( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) noexcept(false); + ValidatedEntityStatus(const ValidatedEntityStatus & other); + ~ValidatedEntityStatus() = default; + + ValidatedEntityStatus() = delete; + ValidatedEntityStatus & operator=(const ValidatedEntityStatus & other) = delete; + ValidatedEntityStatus(ValidatedEntityStatus && other) = delete; + ValidatedEntityStatus & operator=(ValidatedEntityStatus && other) = delete; + + // clang-format off + auto name() const noexcept(true) -> const std::string & { return entity_status_.name; } + auto time() const noexcept(true) -> double { return entity_status_.time; } + auto isLaneletPoseValid() const noexcept(true) -> bool { return entity_status_.lanelet_pose_valid; } + auto pose() const noexcept(true) -> const geometry_msgs::msg::Pose & { return entity_status_.pose; } + auto position() const noexcept(true) -> const geometry_msgs::msg::Point & { return entity_status_.pose.position; } + auto orientation() const noexcept(true) -> const geometry_msgs::msg::Quaternion & { return entity_status_.pose.orientation; } + auto velocity() const noexcept(true) -> const geometry_msgs::msg::Vector3 & { return velocity_; } + auto linearSpeed() const noexcept(true) -> double { return entity_status_.action_status.twist.linear.x; } + auto linearAcceleration() const noexcept(true) -> double { return entity_status_.action_status.accel.linear.x; } + auto boundingBox() const noexcept(true) -> const traffic_simulator_msgs::msg::BoundingBox & { return entity_status_.bounding_box; } + auto behaviorParameter() const noexcept(true) -> const traffic_simulator_msgs::msg::BehaviorParameter & { return behavior_parameter_; } + // clang-format on + + auto buildUpdatedEntityStatus(const geometry_msgs::msg::Vector3 & desired_velocity) const + -> traffic_simulator_msgs::msg::EntityStatus; + +private: + auto validateStepTime(const double step_time) const noexcept(false) -> void; + + auto validatePosition(const geometry_msgs::msg::Point & position) const noexcept(false) -> void; + + auto validateLinearSpeed(const double speed) const noexcept(false) -> void; + + auto validateVelocity(const geometry_msgs::msg::Vector3 & velocity) const noexcept(false) -> void; + + auto validateLinearAcceleration( + const double acceleration, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) const noexcept(false) -> void; + + auto validateBehaviorParameter( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) + -> void; + + template < + typename T, std::enable_if_t::value, std::nullptr_t> = nullptr> + auto throwDetailedValidationError(const std::string & variable_name, const T variable) const + noexcept(false) -> void + { + THROW_SIMULATION_ERROR( + "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status_.name), + ", Variable: ", std::quoted(variable_name), ", variable contains NaN or inf value, ", + "Values: [", variable.x, ", ", variable.y, ", ", variable.z, "]."); + } + + template , std::nullptr_t> = nullptr> + auto throwDetailedValidationError(const std::string & variable_name, const T variable) const + noexcept(false) -> void + { + THROW_SIMULATION_ERROR( + "Error in ValidatedEntityStatus. Entity name: ", std::quoted(entity_status_.name), + ", Variable: ", std::quoted(variable_name), ", variable contains NaN or inf value, ", + "Value: ", variable); + } + + const double step_time_; + const traffic_simulator_msgs::msg::EntityStatus entity_status_; + const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter_; + /// @note velocity_ is the global velocity vector (local velocity rotated using the global orientation). + const geometry_msgs::msg::Vector3 velocity_; +}; + +} // namespace follow_trajectory +} // namespace traffic_simulator + +#endif // TRAFFIC_SIMULATOR__BEHAVIOR__FOLLOW_TRAJECTORY__VALIDATED_ENTITY_STATUS_HPP_ 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 99be9f42930..5dd94d3414d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -221,7 +221,7 @@ class EntityBase : public std::enable_shared_from_this virtual auto requestLaneChange(const lanelet::Id) -> void { /** - * @note There are Entities such as MiscObjectEntity for which lane change is not possible, + * @note There are Entities such as MiscObjectEntity for which lane change is not possible, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } @@ -229,7 +229,7 @@ class EntityBase : public std::enable_shared_from_this virtual auto requestLaneChange(const lane_change::Parameter &) -> void { /** - * @note There are Entities such as MiscObjectEntity for which lane change is not possible, + * @note There are Entities such as MiscObjectEntity for which lane change is not possible, * and since it is necessary to implement appropriate overrides for each Entity, no operation is performed on the base type. */ } diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 8be6682a10a..1dab9111aa9 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -117,6 +117,13 @@ auto distanceToStopLine(Ts &&... xs) return lanelet_wrapper::distance::distanceToStopLine(std::forward(xs)...); } +auto distanceAlongLanelet( + const geometry_msgs::msg::Pose & from_pose, + const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, + const geometry_msgs::msg::Pose & to_pose, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) + -> std::optional; + // spline auto distanceToSpline( const geometry_msgs::msg::Pose & map_pose, diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index dfbe6dab38a..caf850fd210 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -2,7 +2,7 @@ traffic_simulator - 17.5.3 + 17.6.1 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp deleted file mode 100644 index 6e651855cc4..00000000000 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ /dev/null @@ -1,641 +0,0 @@ -// Copyright 2015 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace traffic_simulator -{ -namespace follow_trajectory -{ -template -auto any(F f, T && x, Ts &&... xs) -{ - if constexpr (math::geometry::IsLikeVector3>::value) { - return any(f, x.x, x.y, x.z); - } else if constexpr (0 < sizeof...(xs)) { - return f(x) or any(f, std::forward(xs)...); - } else { - return f(x); - } -} - -auto makeUpdatedStatus( - const traffic_simulator_msgs::msg::EntityStatus & entity_status, - traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const double step_time, - const double matching_distance, std::optional target_speed) -> std::optional -{ - using math::arithmetic::isApproximatelyEqualTo; - using math::arithmetic::isDefinitelyLessThan; - - using math::geometry::operator+; - using math::geometry::operator-; - using math::geometry::operator*; - using math::geometry::operator/; - using math::geometry::operator+=; - - using math::geometry::CatmullRomSpline; - using math::geometry::convertDirectionToQuaternion; - using math::geometry::hypot; - using math::geometry::innerProduct; - using math::geometry::norm; - using math::geometry::normalize; - using math::geometry::truncate; - - constexpr bool include_adjacent_lanelet{false}; - constexpr bool include_opposite_direction{false}; - constexpr bool allow_lane_change{true}; - - const auto include_crosswalk = [](const auto & entity_type) { - return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) || - (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type); - }(entity_status.type); - - auto distance_along_lanelet = - [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { - using geometry_msgs::msg::Pose; - using geometry_msgs::msg::Vector3; - - const RoutingConfiguration routing_configuration{allow_lane_change}; - - const auto quaternion = convertDirectionToQuaternion( - geometry_msgs::build().x(to.x - from.x).y(to.y - from.y).z(to.z - from.z)); - const auto from_pose = geometry_msgs::build().position(from).orientation(quaternion); - if ( - const auto from_canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - from_pose, entity_status.bounding_box, include_crosswalk, matching_distance)) { - const auto to_pose = geometry_msgs::build().position(to).orientation(quaternion); - if ( - const auto to_canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - to_pose, entity_status.bounding_box, include_crosswalk, matching_distance)) { - if (const auto longitudinal_distance = distance::longitudinalDistance( - from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(), - include_adjacent_lanelet, include_opposite_direction, routing_configuration); - longitudinal_distance.has_value() - /** - * DIRTY HACK! - * Negative longitudinal distance (calculated along lanelet in opposite direction) - * causes some scenarios to fail because of an unrelated issue with lanelet matching. - * The issue is caused by wrongly matched lanelet poses and thus wrong distances. - * When lanelet matching errors are fixed, this dirty hack can be removed. - */ - and longitudinal_distance.value() >= 0.0) { - if ( - const auto lateral_distance = distance::lateralDistance( - from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(), - routing_configuration)) { - return std::hypot(longitudinal_distance.value(), lateral_distance.value()); - } - } - } - } - return hypot(from, to); - }; - - auto discard_the_front_waypoint_and_recurse = [&]() { - /* - The OpenSCENARIO standard does not define the behavior when the value of - Timing.domainAbsoluteRelative is "relative". The standard only states - "Definition of time value context as either absolute or relative", and - it is completely unclear when the relative time starts. - - This implementation has interpreted the specification as follows: - Relative time starts from the start of FollowTrajectoryAction or from - the time of reaching the previous "waypoint with arrival time". - - Note: not std::isnan(polyline_trajectory.base_time) means - "Timing.domainAbsoluteRelative is relative". - - Note: not std::isnan(polyline_trajectory.shape.vertices.front().time) - means "The waypoint about to be popped is the waypoint with the - specified arrival time". - */ - if ( - not std::isnan(polyline_trajectory.base_time) and - not std::isnan(polyline_trajectory.shape.vertices.front().time)) { - polyline_trajectory.base_time = entity_status.time; - } - - if (std::rotate( - std::begin(polyline_trajectory.shape.vertices), - std::begin(polyline_trajectory.shape.vertices) + 1, - std::end(polyline_trajectory.shape.vertices)); - not polyline_trajectory.closed) { - polyline_trajectory.shape.vertices.pop_back(); - } - - return makeUpdatedStatus( - entity_status, polyline_trajectory, behavior_parameter, step_time, matching_distance, - 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 - information. - - See https://www.researchgate.net/publication/2495826_Steering_Behaviors_For_Autonomous_Characters - */ - if (polyline_trajectory.shape.vertices.empty()) { - return std::nullopt; - } else if (const auto position = entity_status.pose.position; any(is_infinity_or_nan, position)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), " coordinate value contains NaN or infinity. The value is [", - position.x, ", ", position.y, ", ", position.z, "]."); - } else if ( - /* - We've made sure that polyline_trajectory.shape.vertices is not empty, so - a reference to vertices.front() always succeeds. vertices.front() is - referenced only this once in this member function. - */ - const auto target_position = polyline_trajectory.shape.vertices.front().position.position; - any(is_infinity_or_nan, target_position)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s target position coordinate value contains NaN or infinity. The value is [", - target_position.x, ", ", target_position.y, ", ", target_position.z, "]."); - } else if ( - /* - If not dynamic_constraints_ignorable, the linear distance should cause - problems. - */ - const auto [distance_to_front_waypoint, remaining_time_to_front_waypoint] = std::make_tuple( - distance_along_lanelet(position, target_position), - (not std::isnan(polyline_trajectory.base_time) ? polyline_trajectory.base_time : 0.0) + - polyline_trajectory.shape.vertices.front().time - entity_status.time); - /* - This clause is to avoid division-by-zero errors in later clauses with - distance_to_front_waypoint as the denominator if the distance - miraculously becomes zero. - */ - isDefinitelyLessThan(distance_to_front_waypoint, std::numeric_limits::epsilon())) { - return discard_the_front_waypoint_and_recurse(); - } else if ( - const auto [distance, remaining_time] = - [&]() { - /* - 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 += - distance_along_lanelet(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; - /* - The condition below should ideally be remaining_time < 0. - - The simulator runs at a constant frame rate, so the step time is - 1/FPS. If the simulation time is an accumulation of step times - expressed as rational numbers, times that are integer multiples - of the frame rate will always be exact integer seconds. - Therefore, the timing of remaining_time == 0 always exists, and - the velocity planning of this member function (tick) aims to - reach the waypoint exactly at that timing. So the ideal timeout - condition is remaining_time < 0. - - But actually the step time is expressed as a float and the - simulation time is its accumulation. As a result, it is not - guaranteed that there will be times when the simulation time is - exactly zero. For example, remaining_time == -0.00006 and it was - judged to be out of time. - - For the above reasons, the condition is remaining_time < - -step_time. In other words, the conditions are such that a delay - of 1 step time is allowed. - */ - remaining_time < -step_time) { - throw common::Error( - "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 ", - (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()), - remaining_time != 0 ? remaining_time : std::numeric_limits::epsilon()); - } - } else { - return std::make_tuple( - distance_to_front_waypoint + - total_distance_to(std::end(polyline_trajectory.shape.vertices) - 1), - std::numeric_limits::infinity()); - } - }(); - 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] - std::isinf(acceleration) or std::isnan(acceleration)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), "'s acceleration value is NaN or infinity. The value is ", - acceleration, ". "); - } else if (const auto max_acceleration = std::min( - acceleration /* [m/s^2] */ + - behavior_parameter.dynamic_constraints.max_acceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - +behavior_parameter.dynamic_constraints.max_acceleration /* [m/s^2] */); - std::isinf(max_acceleration) or std::isnan(max_acceleration)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s maximum acceleration value is NaN or infinity. The value is ", max_acceleration, ". "); - } else if (const auto min_acceleration = std::max( - acceleration /* [m/s^2] */ - - behavior_parameter.dynamic_constraints.max_deceleration_rate /* [m/s^3] */ * - step_time /* [s] */, - -behavior_parameter.dynamic_constraints.max_deceleration /* [m/s^2] */); - std::isinf(min_acceleration) or std::isnan(min_acceleration)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s minimum acceleration value is NaN or infinity. The value is ", min_acceleration, ". "); - } else if (const auto speed = entity_status.action_status.twist.linear.x; // [m/s] - std::isinf(speed) or std::isnan(speed)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "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). - - 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 = [&]() -> 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 " - "following information to the developer: Vehicle ", - std::quoted(entity_status.name), - "'s desired acceleration value contains NaN or infinity. The value is ", desired_acceleration, - ". "); - } 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 ", - std::quoted(entity_status.name), "'s desired speed value is NaN or infinity. The value is ", - desired_speed, ". "); - } else if (const auto desired_velocity = - [&]() { - /* - Note: The followingMode in OpenSCENARIO is passed as - variable dynamic_constraints_ignorable. the value of the - variable is `followingMode == position`. - */ - if (polyline_trajectory.dynamic_constraints_ignorable) { - const auto dx = target_position.x - position.x; - const auto dy = target_position.y - position.y; - /// @note if entity is on lane use pitch from lanelet, otherwise use pitch on target - const auto pitch = - entity_status.lanelet_pose_valid - ? -math::geometry::convertQuaternionToEulerAngle( - entity_status.pose.orientation) - .y - : std::atan2(target_position.z - position.z, std::hypot(dy, dx)); - const auto yaw = std::atan2(dy, dx); // Use yaw on target - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * desired_speed) - .y(std::cos(pitch) * std::sin(yaw) * desired_speed) - .z(std::sin(pitch) * desired_speed); - } else { - /* - Note: The vector returned if - dynamic_constraints_ignorable == true ignores parameters - such as the maximum rudder angle of the vehicle entry. In - this clause, such parameters must be respected and the - rotation angle difference of the z-axis center of the - vector must be kept below a certain value. - */ - throw common::SimulationError( - "The followingMode is only supported for position."); - } - }(); - any(is_infinity_or_nan, desired_velocity)) { - throw common::Error( - "An error occurred in the internal state of FollowTrajectoryAction. Please report the " - "following information to the developer: Vehicle ", - 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 (const auto current_velocity = - [&]() { - const auto pitch = - -math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).y; - const auto yaw = - math::geometry::convertQuaternionToEulerAngle(entity_status.pose.orientation).z; - return geometry_msgs::build() - .x(std::cos(pitch) * std::cos(yaw) * speed) - .y(std::cos(pitch) * std::sin(yaw) * speed) - .z(std::sin(pitch) * speed); - }(); - (speed * step_time) > distance_to_front_waypoint && - innerProduct(desired_velocity, current_velocity) < 0.0) { - return discard_the_front_waypoint_and_recurse(); - } else if (auto predicted_state_opt = follow_waypoint_controller.getPredictedWaypointArrivalState( - desired_acceleration, remaining_time, 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, ". ", - follow_waypoint_controller); - } else { - 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; - - std::cout << "acceleration " - << "== " << acceleration - << std::endl; - - std::cout << "min_acceleration " - << "== std::max(acceleration - max_deceleration_rate * step_time, -max_deceleration) " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate << " * " << step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << acceleration << " - " << behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== std::max(" << (acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time) << ", " << -behavior_parameter.dynamic_constraints.max_deceleration << ") " - << "== " << min_acceleration - << std::endl; - - std::cout << "max_acceleration " - << "== std::min(acceleration + max_acceleration_rate * step_time, +max_acceleration) " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate << " * " << step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << acceleration << " + " << behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== std::min(" << (acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time) << ", " << behavior_parameter.dynamic_constraints.max_acceleration << ") " - << "== " << max_acceleration - << std::endl; - - std::cout << "min_acceleration < acceleration < max_acceleration " - << "== " << min_acceleration << " < " << acceleration << " < " << max_acceleration << std::endl; - - std::cout << "desired_acceleration " - << "== 2 * distance / std::pow(remaining_time, 2) - 2 * speed / remaining_time " - << "== 2 * " << distance << " / " << std::pow(remaining_time, 2) << " - 2 * " << speed << " / " << remaining_time << " " - << "== " << (2 * distance / std::pow(remaining_time, 2)) << " - " << (2 * speed / remaining_time) << " " - << "== " << desired_acceleration << " " - << "(acceleration < desired_acceleration == " << (acceleration < desired_acceleration) << " == need to " <<(acceleration < desired_acceleration ? "accelerate" : "decelerate") << ")" - << std::endl; - - std::cout << "desired_speed " - << "== speed + std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time " - << "== " << speed << " + std::clamp(" << desired_acceleration << ", " << min_acceleration << ", " << max_acceleration << ") * " << step_time << " " - << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) << " * " << step_time << " " - << "== " << speed << " + " << std::clamp(desired_acceleration, min_acceleration, max_acceleration) * step_time << " " - << "== " << desired_speed - << std::endl; - - std::cout << "distance_to_front_waypoint " - << "== " << distance_to_front_waypoint - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "== " << remaining_time_to_arrival_to_front_waypoint - << std::endl; - - std::cout << "distance " - << "== " << distance - << std::endl; - - std::cout << "remaining_time " - << "== " << remaining_time - << std::endl; - - std::cout << "remaining_time_to_arrival_to_front_waypoint " - << "(" - << "== distance_to_front_waypoint / desired_speed " - << "== " << distance_to_front_waypoint << " / " << desired_speed << " " - << "== " << remaining_time_to_arrival_to_front_waypoint - << ")" - << std::endl; - - std::cout << "arrive during this frame? " - << "== remaining_time_to_arrival_to_front_waypoint < step_time " - << "== " << remaining_time_to_arrival_to_front_waypoint << " < " << step_time << " " - << "== " << isDefinitelyLessThan(remaining_time_to_arrival_to_front_waypoint, step_time) - << std::endl; - - std::cout << "not too early? " - << "== std::isnan(remaining_time_to_front_waypoint) or remaining_time_to_front_waypoint < remaining_time_to_arrival_to_front_waypoint + step_time " - << "== std::isnan(" << remaining_time_to_front_waypoint << ") or " << remaining_time_to_front_waypoint << " < " << remaining_time_to_arrival_to_front_waypoint << " + " << step_time << " " - << "== " << std::isnan(remaining_time_to_front_waypoint) << " or " << isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time) << " " - << "== " << (std::isnan(remaining_time_to_front_waypoint) or isDefinitelyLessThan(remaining_time_to_front_waypoint, remaining_time_to_arrival_to_front_waypoint + step_time)) - << std::endl; - // clang-format on - } - - if (std::isnan(remaining_time_to_front_waypoint)) { - /// @note If the nearest waypoint is arrived at in this step without a specific arrival time, it will - /// be considered as achieved - if (std::isinf(remaining_time) && polyline_trajectory.shape.vertices.size() == 1) { - /// @note 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 { - /// @note 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), " 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."); - } - } - - /* - Note: If obstacle avoidance is to be implemented, the steering behavior - known by the name "collision avoidance" should be synthesized here into - steering. - */ - auto updated_status = entity_status; - - updated_status.pose.position += desired_velocity * step_time; - - updated_status.pose.orientation = [&]() { - if (desired_velocity.y == 0 && desired_velocity.x == 0 && desired_velocity.z == 0) { - /// @note Do not change orientation if there is no designed_velocity vector - return entity_status.pose.orientation; - } else { - /// @note if there is a designed_velocity vector, set the orientation in the direction of it - return math::geometry::convertDirectionToQuaternion(desired_velocity); - } - }(); - - /// @note If it is the transition between lanelets: overwrite position to improve precision - if (entity_status.lanelet_pose_valid) { - constexpr bool desired_velocity_is_global{true}; - const auto canonicalized_lanelet_pose = - traffic_simulator::pose::toCanonicalizedLaneletPose(entity_status.lanelet_pose); - const auto estimated_next_canonicalized_lanelet_pose = - traffic_simulator::pose::toCanonicalizedLaneletPose(updated_status.pose, include_crosswalk); - if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) { - const auto next_lanelet_id = - static_cast(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id; - if ( /// @note Handle lanelet transition - const auto updated_position = pose::updatePositionForLaneletTransition( - canonicalized_lanelet_pose.value(), next_lanelet_id, desired_velocity, - desired_velocity_is_global, step_time)) { - updated_status.pose.position = updated_position.value(); - } - } - } - - updated_status.action_status.twist.linear.x = norm(desired_velocity); - - updated_status.action_status.twist.linear.y = 0; - - updated_status.action_status.twist.linear.z = 0; - - updated_status.action_status.twist.angular = - math::geometry::convertQuaternionToEulerAngle(math::geometry::getRotation( - entity_status.pose.orientation, updated_status.pose.orientation)) / - step_time; - - updated_status.action_status.accel.linear = - (updated_status.action_status.twist.linear - entity_status.action_status.twist.linear) / - step_time; - - updated_status.action_status.accel.angular = - (updated_status.action_status.twist.angular - entity_status.action_status.twist.angular) / - step_time; - - updated_status.time = entity_status.time + step_time; - - updated_status.lanelet_pose_valid = false; - - return updated_status; - } -} -} // namespace follow_trajectory -} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp new file mode 100644 index 00000000000..8d55903ad94 --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_trajectory.cpp @@ -0,0 +1,117 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ + +/// @note side effects on polyline_trajectory +auto discardTheFrontWaypoint( + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) + -> void; + +/** + * @note follow_trajectory::makeUpdatedEntityStatus is the entry function for the FollowTrajectoryAction behavior. + * + * Initially, a ValidatedEntityStatus is constructed and passed as a parameter. During its construction, + * values such as position, velocity, and step_time are validated. + * + * Then, follow_trajectory::makeUpdatedEntityStatus advances along the trajectory (note the side effects) + * while attempting to obtain a value from PolylineTrajectoryPositioner::makeUpdatedEntityStatus. + * + * If PolylineTrajectoryPositioner::makeUpdatedEntityStatus returns a value, the computation concludes, + * and the value propagates. Notably, PolylineTrajectoryPositioner::makeUpdatedEntityStatus may: + * - return std::nullopt, + * - return an EntityStatus inside std::optional, or + * - throw an exception in cases such as an invalid initial state (failed PolylineTrajectoryPositioner construction) + * or a FollowWaypointController error. + */ +auto makeUpdatedEntityStatus( + const ValidatedEntityStatus & validated_entity_status, + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const double matching_distance, const std::optional target_speed, const double step_time) + -> std::optional +{ + assert(step_time > 0.0); + while (not polyline_trajectory.shape.vertices.empty()) { + const auto updated_entity_opt = + PolylineTrajectoryPositioner( + validated_entity_status, polyline_trajectory, target_speed, matching_distance, step_time) + .makeUpdatedEntityStatus(); + if (updated_entity_opt.has_value()) { + return updated_entity_opt; + } else { + discardTheFrontWaypoint(polyline_trajectory, validated_entity_status.time()); + } + } + return std::nullopt; +} + +auto discardTheFrontWaypoint( + traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double current_time) + -> void +{ + assert(not polyline_trajectory.shape.vertices.empty()); + /** + * @note The OpenSCENARIO standard does not define the behavior when the value of + * Timing.domainAbsoluteRelative is "relative". The standard only states + * "Definition of time value context as either absolute or relative", and + * it is completely unclear when the relative time starts. + * + * This implementation has interpreted the specification as follows: + * Relative time starts from the start of FollowTrajectoryAction or from + * the time of reaching the previous "waypoint with arrival time". + * + * Note: std::isfinite(polyline_trajectory.base_time) means + * "Timing.domainAbsoluteRelative is relative". + * + * Note: std::isfinite(polyline_trajectory.shape.vertices.front().time) + * means "The waypoint about to be popped is the waypoint with the + * specified arrival time". + */ + + if ( + std::isfinite(polyline_trajectory.base_time) and + std::isfinite(polyline_trajectory.shape.vertices.front().time)) { + polyline_trajectory.base_time = current_time; + } + + std::rotate( + std::begin(polyline_trajectory.shape.vertices), + std::begin(polyline_trajectory.shape.vertices) + 1, + std::end(polyline_trajectory.shape.vertices)); + + if (not polyline_trajectory.closed) { + polyline_trajectory.shape.vertices.pop_back(); + } +}; + +} // namespace follow_trajectory +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp similarity index 86% rename from simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp rename to simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp index a85038f69e0..a3e50acee4c 100644 --- a/simulation/traffic_simulator/src/behavior/follow_waypoint_controller.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/follow_waypoint_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include namespace traffic_simulator { @@ -23,7 +23,7 @@ auto FollowWaypointController::getAnalyticalAccelerationForLastSteps( const double speed) const -> double { if (remaining_time <= step_time * 2.0) { - if (!with_breaking || (std::abs(speed) < local_epsilon)) { + if (not with_breaking or (std::abs(speed) < local_epsilon)) { // Step in which the acceleration is set to 0.0. return clampAcceleration(0.0, acceleration, speed); } else { @@ -58,14 +58,14 @@ auto FollowWaypointController::getAnalyticalAccelerationForLastSteps( 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, + step_time * 4.0, "s) in the case of braking and greater than 3 (", step_time * 3.0, "s) without braking."); } auto FollowWaypointController::roundTimeToFullStepsWithTolerance( const double remaining_time_source, const double time_tolerance) const -> double { - if (std::isnan(remaining_time_source) || std::isinf(remaining_time_source)) { + if (not std::isfinite(remaining_time_source)) { throw ControllerError( "Value remaining_time_source (", remaining_time_source, ") is NaN or inf - it cannot be rounded."); @@ -78,7 +78,7 @@ auto FollowWaypointController::roundTimeToFullStepsWithTolerance( " 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; + const double remaining_time = static_cast(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) { @@ -100,7 +100,7 @@ auto FollowWaypointController::getTimeRequiredForNonAcceleration(const double ac auto FollowWaypointController::getAccelerationLimits( const double acceleration, const double speed) const -> std::pair { - const auto time_for_non_acceleration = [&, acceleration]() { + const auto time_for_non_acceleration = [this, acceleration]() { if (std::abs(acceleration) < local_epsilon) { return 0.0; } else { @@ -109,8 +109,9 @@ auto FollowWaypointController::getAccelerationLimits( } }(); - const auto local_min_acceleration = [&]() { - const auto local_min_acceleration = acceleration - max_deceleration_rate * step_time; + const double local_min_acceleration = [this, speed, acceleration, + time_for_non_acceleration]() -> double { + const double 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); @@ -128,8 +129,9 @@ auto FollowWaypointController::getAccelerationLimits( } }(); - const auto local_max_acceleration = [&]() { - const auto local_max_acceleration = acceleration + max_acceleration_rate * step_time; + const double local_max_acceleration = [this, speed, acceleration, time_for_non_acceleration, + local_min_acceleration]() -> double { + const double 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); @@ -160,8 +162,8 @@ auto FollowWaypointController::getAccelerationLimits( 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 > std::max(max_speed, target_speed) + local_epsilon || - speed_min < -local_epsilon || speed_min > std::max(max_speed, target_speed) + local_epsilon) { + speed_max < -local_epsilon or speed_max > std::max(max_speed, target_speed) + local_epsilon or + speed_min < -local_epsilon or speed_min > std::max(max_speed, target_speed) + local_epsilon) { throw ControllerError( "Incorrect acceleration limits [", local_min_acceleration, ", ", local_max_acceleration, "] for acceleration: ", acceleration, " and speed: ", speed, " -> speed_min: ", speed_min, @@ -176,7 +178,7 @@ auto FollowWaypointController::clampAcceleration( const double candidate_acceleration, const double acceleration, const double speed) const -> double { - auto [local_min_acceleration, local_max_acceleration] = + const auto [local_min_acceleration, local_max_acceleration] = getAccelerationLimits(acceleration, speed); return std::clamp(candidate_acceleration, local_min_acceleration, local_max_acceleration); } @@ -194,7 +196,7 @@ auto FollowWaypointController::getPredictedStopStateWithoutConsideringTime( { PredictedState breaking_check{acceleration, speed, 0.0, 0.0}; moveStraight(breaking_check, step_acceleration); - while (!breaking_check.isImmobile(local_epsilon)) { + while (not breaking_check.isImmobile(local_epsilon)) { if (breaking_check.traveled_distance > remaining_distance + predicted_distance_tolerance) { return std::nullopt; } else { @@ -208,8 +210,8 @@ 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)) { + const auto brakeUntilImmobility = [this, remaining_time](PredictedState & state) -> bool { + while (not state.isImmobile(local_epsilon)) { if (state.travel_time >= remaining_time) { return false; } else { @@ -230,7 +232,7 @@ auto FollowWaypointController::getPredictedWaypointArrivalState( if (with_breaking) { // Predict the current (before acceleration zeroing) braking time required for stopping. PredictedState breaking_check = state; - if (!brakeUntilImmobility(breaking_check)) { + if (not 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) { @@ -262,7 +264,7 @@ auto FollowWaypointController::getPredictedWaypointArrivalState( if (with_breaking) { // Predict the current (after acceleration zeroing) braking time required for stopping. - if (!brakeUntilImmobility(state)) { + if (not 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) { @@ -306,14 +308,15 @@ auto FollowWaypointController::getAcceleration( 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; + for (std::size_t i = 0UL; i <= number_of_acceleration_candidates; ++i) { + const double candidate_acceleration = + local_min_acceleration + static_cast(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 || min_distance_diff < 0) && + if (const double distance_diff = remaining_distance - predicted_state_opt->traveled_distance; + (distance_diff >= 0.0 or min_distance_diff < 0.0) && (std::abs(distance_diff) < std::abs(min_distance_diff))) { min_distance_diff = distance_diff; best_acceleration = candidate_acceleration; @@ -363,10 +366,10 @@ auto FollowWaypointController::getAcceleration( /* If last steps, increase accuracy by using analytical calculations. */ - if (with_breaking && remaining_time <= step_time * 4) { + if (with_breaking && remaining_time <= step_time * 4.0) { return getAnalyticalAccelerationForLastSteps( remaining_time, remaining_distance, acceleration, speed); - } else if (remaining_time <= step_time * 3) { + } else if (remaining_time <= step_time * 3.0) { return getAnalyticalAccelerationForLastSteps( remaining_time, remaining_distance, acceleration, speed); } @@ -378,16 +381,19 @@ auto FollowWaypointController::getAcceleration( &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 time_better = not same_time and (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); + distance_diff >= 0.0 and + (std::abs(distance_diff) < std::abs(min_distance_diff) or min_distance_diff < 0.0); + return time_better or (same_time and distance_better); }; // Create a lambda for candidate selection std::optional best_acceleration = std::nullopt; - const auto considerCandidate = [&](double candidate_acceleration) -> void { + const auto considerCandidate = + [this, remaining_time, remaining_distance, acceleration, speed, isBetterCandidate, + &min_time_diff, &min_distance_diff, + &best_acceleration](const double candidate_acceleration) -> void { if ( const auto predicted_state_opt = getPredictedWaypointArrivalState( candidate_acceleration, remaining_time, remaining_distance, acceleration, speed)) { @@ -403,7 +409,7 @@ auto FollowWaypointController::getAcceleration( // Consider the borderline values and precise value of 0 considerCandidate(local_min_acceleration); - if (local_min_acceleration < 0.0 && local_max_acceleration > 0.0) { + if (local_min_acceleration < 0.0 and local_max_acceleration > 0.0) { considerCandidate(0.0); } considerCandidate(local_max_acceleration); @@ -420,8 +426,8 @@ auto FollowWaypointController::getAcceleration( 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); + for (std::size_t i = 1UL; i < number_of_acceleration_candidates; ++i) { + considerCandidate(local_min_acceleration + static_cast(i) * step_acceleration); } } @@ -431,7 +437,7 @@ auto FollowWaypointController::getAcceleration( } 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) { + std::abs(min_time_diff) < step_time and 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 " diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp new file mode 100644 index 00000000000..05e86849c9d --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/polyline_trajectory_positioner.cpp @@ -0,0 +1,468 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ + +PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( + const ValidatedEntityStatus & validated_entity_status, + const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, + const std::optional target_speed, const double matching_distance, const double step_time) +: validated_entity_status_(validated_entity_status), + polyline_trajectory_(polyline_trajectory), + step_time_(step_time), + matching_distance_(matching_distance), + nearest_waypoint_with_specified_time_it_( + getNearestWaypointWithSpecifiedTimeIterator()), // implicitly requires: polyline_trajectory_ + nearest_waypoint_pose_(validatedEntityTargetPose()), // implicitly requires: polyline_trajectory_ + distance_to_nearest_waypoint_( + validatedDistanceToNearestWaypoint()), //implicitly requires: nearest_waypoint_pose_, validated_entity_status_, hdmap_utils_ptr_, + total_remaining_distance_( + validatedTotalRemainingDistance()), // implicitly requires: polyline_trajectory_, hdmap_utils_ptr_, matching_distance_ + time_to_nearest_waypoint_(timeToNearestWaypoint()), + total_remaining_time_( + validatedTotalRemainingTime()), // implicitly requires: nearest_waypoint_with_specified_time_it_, polyline_trajectory_, validated_entity_status_, step_time_ + follow_waypoint_controller_( + validated_entity_status_.behaviorParameter(), step_time_, + isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint(), // implicitly requires: nearest_waypoint_with_specified_time_it_, polyline_trajectory_ + std::isfinite(total_remaining_time_) ? std::nullopt : target_speed) +{ +} + +auto PolylineTrajectoryPositioner::makeUpdatedEntityStatus() const -> std::optional +{ + const auto desired_local_acceleration = validatedEntityDesiredLinearAcceleration(); + const auto desired_speed = validatedEntityDesiredSpeed(desired_local_acceleration); + const auto desired_local_velocity = validatedEntityDesiredLocalVelocity(desired_speed); + + /// @note cancel step if is close to the nearest waypoint and requires a large change of direction + if ( + validated_entity_status_.linearSpeed() * step_time_ > distance_to_nearest_waypoint_ and + math::geometry::innerProduct(desired_local_velocity, validated_entity_status_.velocity()) < + 0.0) { + return std::nullopt; + } + + validatePredictedState(desired_local_acceleration); + if (isNearestWaypointReachable(desired_local_acceleration)) { + return validated_entity_status_.buildUpdatedEntityStatus(desired_local_velocity); + } else { + return std::nullopt; + } +} + +auto PolylineTrajectoryPositioner::getWaypoints() const noexcept(true) -> const Waypoints & +{ + return polyline_trajectory_.shape.vertices; +} + +auto PolylineTrajectoryPositioner::getNearestWaypointWithSpecifiedTimeIterator() const + -> WaypointIterator +{ + return std::find_if(getWaypoints().cbegin(), getWaypoints().cend(), [](const auto & vertex) { + return std::isfinite(vertex.time); + }); +} + +auto PolylineTrajectoryPositioner::getTimeToWaypoint(const Waypoint & waypoint) const -> double +{ + const double time_difference = waypoint.time - validated_entity_status_.time(); + if (isAbsoluteBaseTime()) { + return ABSOLUTE_BASE_TIME + time_difference; + } else { + return polyline_trajectory_.base_time + time_difference; + } +} + +auto PolylineTrajectoryPositioner::areConditionsOfArrivalMet() const noexcept(true) -> bool +{ + return follow_waypoint_controller_.areConditionsOfArrivalMet( + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed(), + distance_to_nearest_waypoint_); +} + +auto PolylineTrajectoryPositioner::isAbsoluteBaseTime() const noexcept(true) -> bool +{ + return not std::isfinite(polyline_trajectory_.base_time); +} +/** + * @note + * The follow_waypoint_controller calculates accelerations based on the timestamps (arrival time) + * defined in waypoints. isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() determines if braking + * should be taken into account - it return true if the nearest waypoint with a specified time is + * the last waypoint or there is no timed 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. + */ +auto PolylineTrajectoryPositioner::isNearestWaypointWithSpecifiedTimeSameAsLastWaypoint() const + -> bool +{ + return nearest_waypoint_with_specified_time_it_ >= std::prev(getWaypoints().cend()); +} + +/** + * @note isNearestWaypointReachable works as follows + * + * 1. if **Nearest waypoint without a timestamp:** + * - 1.1 If it is the last waypoint: + * - Checks if the arrival conditions are met. If so, ends the movement (return false). + * - Otherwise, updates and returns the status with a new velocity. + * - 1.2 If it is an intermediate waypoint: + * - Checks if it can be reached within the current step. + * If so, considers it as reached (return false). + * - Otherwise, updates and returns the status with a new velocity. + * + * 2. else **Nearest waypoint with a timestamp:** + * - 2.1 If the waypoint should be reached in this step (time remaining is less than step time/2): + * - Checks if the arrival conditions are met. If so, ends the movement (return false). + * - Throws an error if the distance exceeds the accepted accuracy. + * - 2.2 If the waypoint should not be reached in this step: + * - Updates and returns the status with a new velocity. + * + * Note 2.1, about step_time_ / 2.0: + * 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 (time_to_nearest_waypoint is almost zero). + */ +auto PolylineTrajectoryPositioner::isNearestWaypointReachable( + const double desired_local_acceleration) const -> bool +{ + if (not std::isfinite(time_to_nearest_waypoint_)) { + if (getWaypoints().size() == 1UL) { + if (areConditionsOfArrivalMet()) { + return false; + } else { + return true; + } + } else { + if (const double this_step_speed = + (validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_); + this_step_speed * step_time_ > distance_to_nearest_waypoint_) { + return false; + } else { + return true; + } + } + } else { + if (math::arithmetic::isDefinitelyLessThan(time_to_nearest_waypoint_, step_time_ / 2.0)) { + if (areConditionsOfArrivalMet()) { + return false; + } else { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status_.name()), " at time ", + validated_entity_status_.time(), "s (remaining time is ", time_to_nearest_waypoint_, + "s), has completed a trajectory to the nearest waypoint with specified time equal to ", + getWaypoints().front().time, "s at a distance equal to ", distance_to_nearest_waypoint_, + " from that waypoint which is greater than the accepted accuracy."); + } + } else { + return true; + } + } +} + +auto PolylineTrajectoryPositioner::validatedDistanceToNearestWaypoint() const -> double +{ + const double distance_euclidean = + math::geometry::hypot(validated_entity_status_.position(), nearest_waypoint_pose_.position); + if (const std::optional opt_distance_along_lanelet = distance::distanceAlongLanelet( + validated_entity_status_.pose(), validated_entity_status_.boundingBox(), + nearest_waypoint_pose_, validated_entity_status_.boundingBox(), matching_distance_); + opt_distance_along_lanelet.has_value()) { + if (opt_distance_along_lanelet.value() < 0.0) { + /// @note FollowTrajectoryAction does not support backwards movement + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status_.name()), + "'s distance_to_nearest_waypoint is less than 0.0. The value is ", + opt_distance_along_lanelet.value(), "."); + } else if (opt_distance_along_lanelet.value() <= matching_distance_) { + /// @note If the waypoint is this close - use euclidean distance to avoid inaccuracies related to lanelet matching + return distance_euclidean; + } else { + return opt_distance_along_lanelet.value(); + } + } else { + return distance_euclidean; + } +} + +auto PolylineTrajectoryPositioner::validatedTotalRemainingDistance() const -> double +{ + /** + * @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. + */ + const auto total_distance_to = [this](const WaypointIterator last) { + return std::accumulate( + getWaypoints().cbegin(), last, 0.0, + [this](const double total_distance, const Waypoint & vertex) { + const auto next_vertex = std::next(&vertex); + return total_distance + distance::distanceAlongLanelet( + vertex.position, validated_entity_status_.boundingBox(), + next_vertex->position, validated_entity_status_.boundingBox(), + matching_distance_) + .value_or(math::geometry::hypot( + vertex.position.position, next_vertex->position.position)); + }); + }; + + const double total_remaining_distance = [&] { + if (nearest_waypoint_with_specified_time_it_ == getWaypoints().cend()) { + return distance_to_nearest_waypoint_ + total_distance_to(std::prev(getWaypoints().cend())); + } else { + return distance_to_nearest_waypoint_ + + total_distance_to(nearest_waypoint_with_specified_time_it_); + } + }(); + + /// @note FollowTrajectoryAction does not support backwards movement + if (total_remaining_distance < 0.0) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status_.name()), + "'s total_remaining_distance is less than 0.0. The value is ", total_remaining_distance, "."); + } else { + return total_remaining_distance; + } +} + +auto PolylineTrajectoryPositioner::timeToNearestWaypoint() const noexcept(false) -> double +{ + if (not std::isfinite(getWaypoints().front().time)) { + return std::numeric_limits::infinity(); + } else { + return getTimeToWaypoint(getWaypoints().front()); + } +}; + +auto PolylineTrajectoryPositioner::validatedTotalRemainingTime() const noexcept(false) -> double +{ + if (nearest_waypoint_with_specified_time_it_ == getWaypoints().cend()) { + return std::numeric_limits::infinity(); + } else { + const double remaining_time = getTimeToWaypoint(*nearest_waypoint_with_specified_time_it_); + /** + * @note The condition below should ideally be remaining_time < 0. + * + * The simulator runs at a constant frame rate, so the step time is + * 1/FPS. If the simulation time is an accumulation of step times + * expressed as rational numbers, times that are integer multiples + * of the frame rate will always be exact integer seconds. + * Therefore, the timing of remaining_time == 0 always exists, and + * the velocity planning of this member function (tick) aims to + * reach the waypoint exactly at that timing. So the ideal timeout + * condition is remaining_time < 0. + * + * But actually the step time is expressed as a float and the + * simulation time is its accumulation. As a result, it is not + * guaranteed that there will be times when the simulation time is + * exactly zero. For example, remaining_time == -0.00006 and it was + * judged to be out of time. + * + * For the above reasons, the condition is remaining_time < + * -step_time. In other words, the conditions are such that a delay + * of 1 step time is allowed. + */ + + if (remaining_time < -step_time_) { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status_.name()), + " failed to reach the trajectory waypoint at the specified time. The specified time " + "is ", + nearest_waypoint_with_specified_time_it_->time, " (in ", + (isAbsoluteBaseTime() ? "absolute" : "relative"), + " simulation time). This may be due to unrealistic conditions of arrival time " + "specification compared to vehicle parameters and dynamic constraints."); + } else { + return remaining_time == 0.0 ? std::numeric_limits::epsilon() : remaining_time; + } + } +} + +auto PolylineTrajectoryPositioner::validatedEntityDesiredLocalVelocity( + const double desired_speed) const noexcept(false) -> geometry_msgs::msg::Vector3 +{ + /** + * @note The followingMode in OpenSCENARIO is passed as variable dynamic_constraints_ignorable. + * the value of the variable is `followingMode == position`. + */ + if (not polyline_trajectory_.dynamic_constraints_ignorable) { + /** + * @note The vector returned if dynamic_constraints_ignorable == true ignores parameters + * such as the maximum rudder angle of the vehicle entry. In this clause, such parameters + * must be respected and the rotation angle difference of the z-axis center of the + * vector must be kept below a certain value. + */ + THROW_SIMULATION_ERROR("The followingMode is only supported for position."); + } + + const double dx = nearest_waypoint_pose_.position.x - validated_entity_status_.position().x; + const double dy = nearest_waypoint_pose_.position.y - validated_entity_status_.position().y; + + /// @note if entity is on lane use pitch from lanelet, otherwise use pitch on target + const double pitch = + validated_entity_status_.isLaneletPoseValid() + ? -math::geometry::convertQuaternionToEulerAngle(validated_entity_status_.orientation()).y + : std::atan2( + nearest_waypoint_pose_.position.z - validated_entity_status_.position().z, + std::hypot(dy, dx)); + /// @note use yaw on target + const double yaw = std::atan2(dy, dx); + + const auto desired_local_velocity = geometry_msgs::build() + .x(std::cos(pitch) * std::cos(yaw) * desired_speed) + .y(std::cos(pitch) * std::sin(yaw) * desired_speed) + .z(std::sin(pitch) * desired_speed); + + if (math::geometry::isFinite(desired_local_velocity)) { + return desired_local_velocity; + } else { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status_.name()), + "'s desired velocity contains NaN or infinity. The value is [", desired_local_velocity.x, + ", ", desired_local_velocity.y, ", ", desired_local_velocity.z, "]."); + } +} + +auto PolylineTrajectoryPositioner::validatedEntityDesiredLinearAcceleration() const noexcept(false) + -> double +{ + /** + * @note The desired acceleration is the acceleration to reach the nearest_waypoint (as accurately as possible) + * in the specified time (total_remaining_time_as close as possible to 0.0). + * + * 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. + */ + + try { + const double desired_linear_acceleration = follow_waypoint_controller_.getAcceleration( + total_remaining_time_, total_remaining_distance_, + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed()); + + if (std::isfinite(desired_linear_acceleration)) { + return desired_linear_acceleration; + } else { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status_.name()), + "'s desired acceleration value contains NaN or infinity. The value is ", + desired_linear_acceleration, ". "); + } + + } catch (const ControllerError & e) { + THROW_SIMULATION_ERROR( + "Vehicle ", std::quoted(validated_entity_status_.name()), + " - controller operation problem encountered. ", + follow_waypoint_controller_.getFollowedWaypointDetails(polyline_trajectory_), e.what()); + } +} + +auto PolylineTrajectoryPositioner::validatedEntityTargetPose() const noexcept(false) + -> geometry_msgs::msg::Pose +{ + if (getWaypoints().empty()) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "attempted to dereference an element of an empty PolylineTrajectory for vehicle ", + std::quoted(validated_entity_status_.name())); + } else if (const auto & target_pose = getWaypoints().front().position; + not math::geometry::isFinite(target_pose.position)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status_.name()), + "'s target position coordinate value contains NaN or infinity. The value is [", + target_pose.position.x, ", ", target_pose.position.y, ", ", target_pose.position.z, "]."); + } else { + return target_pose; + } +} + +auto PolylineTrajectoryPositioner::validatedEntityDesiredSpeed( + const double desired_local_acceleration) const noexcept(false) -> double +{ + if (const double desired_speed = + validated_entity_status_.linearSpeed() + desired_local_acceleration * step_time_; + not std::isfinite(desired_speed)) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: Vehicle ", + std::quoted(validated_entity_status_.name()), + "'s desired speed value is NaN or infinity. The value is ", desired_speed, ". "); + } else { + return desired_speed; + } +} + +auto PolylineTrajectoryPositioner::validatePredictedState( + const double desired_local_acceleration) const noexcept(false) -> void +{ + if (const auto predicted_state = follow_waypoint_controller_.getPredictedWaypointArrivalState( + desired_local_acceleration, total_remaining_time_, total_remaining_distance_, + validated_entity_status_.linearAcceleration(), validated_entity_status_.linearSpeed()); + not std::isinf(total_remaining_time_) and not predicted_state.has_value()) { + THROW_SIMULATION_ERROR( + "An error occurred in the internal state of FollowTrajectoryAction. Please report the " + "following information to the developer: FollowWaypointController for vehicle ", + std::quoted(validated_entity_status_.name()), " calculated invalid acceleration:", + " desired_local_acceleration: ", desired_local_acceleration, + ", total_remaining_time: ", total_remaining_time_, + ", total_remaining_distance: ", total_remaining_distance_, + ", acceleration: ", validated_entity_status_.linearAcceleration(), + ", speed: ", validated_entity_status_.linearSpeed(), ". ", follow_waypoint_controller_); + } +} +} // namespace follow_trajectory +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp new file mode 100644 index 00000000000..cd5776e9a77 --- /dev/null +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory/validated_entity_status.cpp @@ -0,0 +1,225 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace follow_trajectory +{ + +ValidatedEntityStatus::ValidatedEntityStatus( + const traffic_simulator_msgs::msg::EntityStatus & entity_status, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) noexcept(false) +: step_time_(step_time), + entity_status_(entity_status), + behavior_parameter_(behavior_parameter), + velocity_(math::geometry::rotate( + entity_status_.action_status.twist.linear, entity_status_.pose.orientation)) +{ + validateStepTime(step_time_); + validateBehaviorParameter(behaviorParameter()); + validatePosition(position()); + validateLinearSpeed(linearSpeed()); + validateVelocity(velocity()); + validateLinearAcceleration(linearAcceleration(), behaviorParameter(), step_time_); +} + +ValidatedEntityStatus::ValidatedEntityStatus(const ValidatedEntityStatus & other) +: ValidatedEntityStatus(other.entity_status_, other.behavior_parameter_, other.step_time_) +{ +} + +/// @brief this method updates the EntityStatus by applying a local velocity vector, +/// during the update the position on the lanelet is lost (it is set as invalid) +auto ValidatedEntityStatus::buildUpdatedEntityStatus( + const geometry_msgs::msg::Vector3 & desired_local_velocity) const + -> traffic_simulator_msgs::msg::EntityStatus +{ + using math::geometry::operator+; + using math::geometry::operator-; + using math::geometry::operator*; + using math::geometry::operator/; + + constexpr bool desired_velocity_is_global = true; + + const auto include_crosswalk = [](const auto & entity_type) { + return (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == entity_type.type) || + (traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == entity_type.type); + }(entity_status_.type); + + const auto updated_time = entity_status_.time + step_time_; + + const auto updated_pose_orientation = + desired_local_velocity == geometry_msgs::msg::Vector3() + ? entity_status_.pose.orientation + : math::geometry::convertDirectionToQuaternion(desired_local_velocity); + + auto updated_pose = + geometry_msgs::build() + .position(entity_status_.pose.position + desired_local_velocity * step_time_) + .orientation(updated_pose_orientation); + + /// @note If it is the transition between lanelets: overwrite position to improve precision + if (entity_status_.lanelet_pose_valid) { + const auto canonicalized_lanelet_pose = + pose::toCanonicalizedLaneletPose(entity_status_.lanelet_pose); + const auto estimated_next_canonicalized_lanelet_pose = + pose::toCanonicalizedLaneletPose(updated_pose, include_crosswalk); + if (canonicalized_lanelet_pose && estimated_next_canonicalized_lanelet_pose) { + const auto next_lanelet_id = + static_cast(estimated_next_canonicalized_lanelet_pose.value()).lanelet_id; + if ( + const auto updated_position = pose::updatePositionForLaneletTransition( + canonicalized_lanelet_pose.value(), next_lanelet_id, desired_local_velocity, + desired_velocity_is_global, step_time_)) { + updated_pose.position = updated_position.value(); + } + } + } + + const auto updated_twist_linear = geometry_msgs::build() + .x(math::geometry::norm(desired_local_velocity)) + .y(0.0) + .z(0.0); + const auto updated_twist_angular = + math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(entity_status_.pose.orientation, updated_pose_orientation)) / + step_time_; + const auto updated_twist = geometry_msgs::build() + .linear(updated_twist_linear) + .angular(updated_twist_angular); + + const auto updated_acceleration = + geometry_msgs::build() + .linear((updated_twist_linear - entity_status_.action_status.twist.linear) / step_time_) + .angular((updated_twist_angular - entity_status_.action_status.twist.angular) / step_time_); + + const auto updated_linear_jerk = + (updated_acceleration.linear.x - entity_status_.action_status.accel.linear.x) / step_time_; + + const auto updated_action_status = + traffic_simulator_msgs::build() + .current_action(entity_status_.action_status.current_action) + .twist(updated_twist) + .accel(updated_acceleration) + .linear_jerk(updated_linear_jerk); + + return traffic_simulator_msgs::build() + .type(entity_status_.type) + .subtype(entity_status_.subtype) + .time(updated_time) + .name(entity_status_.name) + .bounding_box(entity_status_.bounding_box) + .action_status(updated_action_status) + .pose(updated_pose) + .lanelet_pose(traffic_simulator_msgs::msg::LaneletPose()) + .lanelet_pose_valid(false); +} + +auto ValidatedEntityStatus::validateStepTime(const double step_time) const noexcept(false) -> void +{ + static constexpr double step_time_tolerance = 1e-6; + if (step_time <= step_time_tolerance) { + throwDetailedValidationError("step_time", step_time); + } +} + +auto ValidatedEntityStatus::validatePosition(const geometry_msgs::msg::Point & position) const + noexcept(false) -> void +{ + if (not math::geometry::isFinite(position)) { + throwDetailedValidationError("position", position); + } +} + +auto ValidatedEntityStatus::validateLinearSpeed(const double speed) const noexcept(false) -> void +{ + if (not std::isfinite(speed)) { + throwDetailedValidationError("speed", speed); + } +} + +auto ValidatedEntityStatus::validateVelocity(const geometry_msgs::msg::Vector3 & velocity) const + noexcept(false) -> void +{ + if (not math::geometry::isFinite(velocity)) { + throwDetailedValidationError("velocity", velocity); + } +} + +auto ValidatedEntityStatus::validateLinearAcceleration( + const double acceleration, + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, + const double step_time) const noexcept(false) -> void +{ + const double max_acceleration = std::min( + acceleration + behavior_parameter.dynamic_constraints.max_acceleration_rate * step_time, + +behavior_parameter.dynamic_constraints.max_acceleration); + const double min_acceleration = std::max( + acceleration - behavior_parameter.dynamic_constraints.max_deceleration_rate * step_time, + -behavior_parameter.dynamic_constraints.max_deceleration); + if (not std::isfinite(acceleration)) { + throwDetailedValidationError("acceleration", acceleration); + } else if (not std::isfinite(max_acceleration)) { + throwDetailedValidationError("maximum acceleration", max_acceleration); + } else if (not std::isfinite(min_acceleration)) { + throwDetailedValidationError("minimum acceleration", min_acceleration); + } +} + +auto ValidatedEntityStatus::validateBehaviorParameter( + const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter) const noexcept(false) + -> void +{ + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration_rate)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration_rate)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_acceleration)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } + if (not std::isfinite(behavior_parameter.dynamic_constraints.max_deceleration)) { + throwDetailedValidationError( + "behavior_parameter.dynamic_constraints.max_acceleration_rate", + behavior_parameter.dynamic_constraints.max_acceleration_rate); + } +} +} // namespace follow_trajectory +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 99e57113061..7f68d17dbb5 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -179,15 +179,20 @@ auto EgoEntity::updateFieldOperatorApplication() -> void { spinSome(); } void EgoEntity::onUpdate(double current_time, double step_time) { - EntityBase::onUpdate(current_time, step_time); + const auto getTargetSpeed = [this]() -> double { + return target_speed_.value_or(status_->getTwist().linear.x); + }; + EntityBase::onUpdate(current_time, step_time); if (is_controlled_by_simulator_) { - if ( - const auto non_canonicalized_updated_status = - traffic_simulator::follow_trajectory::makeUpdatedStatus( - static_cast(*status_), *polyline_trajectory_, - behavior_parameter_, step_time, getDefaultMatchingDistanceForLaneletPoseCalculation(), - target_speed_ ? target_speed_.value() : status_->getTwist().linear.x)) { + if (const auto non_canonicalized_updated_status = + traffic_simulator::follow_trajectory::makeUpdatedEntityStatus( + traffic_simulator::follow_trajectory::ValidatedEntityStatus( + static_cast(*status_), behavior_parameter_, + step_time), + *polyline_trajectory_, getDefaultMatchingDistanceForLaneletPoseCalculation(), + getTargetSpeed(), step_time); + non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side setStatus(non_canonicalized_updated_status.value(), status_->getLaneletIds()); } else { diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index ca2e65d0314..dd7afcd0918 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace traffic_simulator @@ -94,7 +95,7 @@ auto longitudinalDistance( /** * @brief A matching distance of about 1.5*lane widths is given as the matching distance to match the * Entity present on the adjacent Lanelet. - * The length of the horizontal bar must intersect with the adjacent lanelet, + * The length of the horizontal bar must intersect with the adjacent lanelet, * so it is always 10m regardless of the entity type. */ constexpr double matching_distance = 5.0; @@ -323,6 +324,52 @@ auto distanceToCrosswalk( } } +auto distanceAlongLanelet( + const geometry_msgs::msg::Pose & from_pose, + const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, + const geometry_msgs::msg::Pose & to_pose, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, const double matching_distance) + -> std::optional +{ + /// @note original source code: https://github.com/tier4/scenario_simulator_v2/blob/e0b2c122a5ff4e592a7ee68be6bb150f0512d44b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp#L80-L119 + /// @note due to this hardcoded value, the method cannot be used for calculations along a crosswalk (for pedestrians) + constexpr bool include_crosswalk = false; + constexpr bool include_adjacent_lanelet = false; + constexpr bool include_opposite_direction = false; + constexpr bool allow_lane_change = true; + + const auto routing_configuration = traffic_simulator::RoutingConfiguration{allow_lane_change}; + + if (const auto from_canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + from_pose, from_bounding_box, include_crosswalk, matching_distance); + from_canonicalized_lanelet_pose.has_value()) { + if (const auto to_canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + to_pose, to_bounding_box, include_crosswalk, matching_distance); + to_canonicalized_lanelet_pose.has_value()) { + if (const auto longitudinal_distance = traffic_simulator::distance::longitudinalDistance( + from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(), + include_adjacent_lanelet, include_opposite_direction, routing_configuration); + longitudinal_distance.has_value() + /** + * DIRTY HACK! + * Negative longitudinal distance (calculated along lanelet in opposite direction) + * causes some scenarios to fail because of an unrelated issue with lanelet matching. + * The issue is caused by wrongly matched lanelet poses and thus wrong distances. + * When lanelet matching errors are fixed, this dirty hack can be removed. + */ + and longitudinal_distance.value() >= 0.0) { + if ( + const auto lateral_distance = distance::lateralDistance( + from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(), + routing_configuration)) { + return std::hypot(longitudinal_distance.value(), lateral_distance.value()); + } + } + } + } + return std::nullopt; +} + auto distanceToSpline( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 5f55b0fda6a..a5524c736b2 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index d4e18e05216..c1477434ea9 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package openscenario_msgs 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 65e4d98f6fc..c6b98656012 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 17.5.3 + 17.6.1 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 b0b433703f2..f0e7cd19bf7 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -24,6 +24,14 @@ Changelog for package random_test_runner 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge branch 'master' into fix/noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index c135256a045..1e3083a14d7 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 17.5.3 + 17.6.1 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index eee2302456d..b203158f87f 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -38,6 +38,19 @@ Changelog for package scenario_test_runner 16.4.2 (2025-05-23) ------------------- +17.6.1 (2025-10-02) +------------------- + +17.6.0 (2025-09-29) +------------------- +* Merge pull request `#1685 `_ from tier4/fix/noise_v4 +* Merge branch 'master' into fix/noise_v4 +* use elliptical model for x,y,yaw in noise_v4 +* use same true positive calculation algorithm to noise_v2 in noise_v4 +* rename noise.v4..rotation into noise.v4..yaw +* Implement yaw_flip noise for noise_v4 +* Contributors: Kotaro Yoshimoto + 17.5.3 (2025-09-17) ------------------- * Merge branch 'master' into fix/macro-xml-validation diff --git a/test_runner/scenario_test_runner/config/parameters.yaml b/test_runner/scenario_test_runner/config/parameters.yaml index 09cb4429911..3c7ff615d87 100644 --- a/test_runner/scenario_test_runner/config/parameters.yaml +++ b/test_runner/scenario_test_runner/config/parameters.yaml @@ -232,36 +232,56 @@ types: ["*"] subtypes: ["*"] names: ["*"] + ellipse_y_radii: [ 10.0, 20.0, 40.0, 60.0, 80.0, 120.0, 150.0, 180.0, 1000.0 ] position: x: autocorrelation_coefficient: amplitude: 0.0 decay: 0.0 offset: 0.0 - mean: 0.0 - standard_deviation: 0.0 + mean: + ellipse_normalized_x_radius: 1.0 + values: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] + standard_deviation: + ellipse_normalized_x_radius: 1.0 + values: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] y: autocorrelation_coefficient: amplitude: 0.0 decay: 0.0 offset: 0.0 - mean: 0.0 - standard_deviation: 0.0 - rotation: + mean: + ellipse_normalized_x_radius: 1.0 + values: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + standard_deviation: + ellipse_normalized_x_radius: 1.0 + values: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + yaw: + autocorrelation_coefficient: + amplitude: 0.0 + decay: 0.0 + offset: 0.0 + mean: + ellipse_normalized_x_radius: 1.0 + values: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + standard_deviation: + ellipse_normalized_x_radius: 1.0 + values: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] + yaw_flip: autocorrelation_coefficient: amplitude: 0.0 decay: 0.0 offset: 0.0 - mean: 0.0 - standard_deviation: 0.0 + speed_threshold: 0.1 + rate: 0.0 true_positive: autocorrelation_coefficient: amplitude: 0.0 decay: 0.0 offset: 0.0 rate: - distance_thresholds: [ 1000.0 ] - values: [ 1.0 ] + ellipse_normalized_x_radius: 1.0 + values: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] /perception/object_recognition/ground_truth/objects: version: 20240605 # architecture_type suffix (mandatory) override_legacy_configuration: false diff --git a/test_runner/scenario_test_runner/config/workflow.txt b/test_runner/scenario_test_runner/config/workflow.txt index b5ae1aef8e6..65846f86a52 100644 --- a/test_runner/scenario_test_runner/config/workflow.txt +++ b/test_runner/scenario_test_runner/config/workflow.txt @@ -27,3 +27,7 @@ $(find-pkg-share scenario_test_runner)/scenario/EntitySelection/SpeedAction_Spee $(find-pkg-share scenario_test_runner)/scenario/EntitySelection/TeleportAction_CollisionCondition_StandStillCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/EntitySelection/TimeHeadwayCondition.yaml $(find-pkg-share scenario_test_runner)/scenario/collision_condition_by_type.yaml +$(find-pkg-share scenario_test_runner)/scenario/FTA-only_destination_with_time_constraint.yaml +$(find-pkg-share scenario_test_runner)/scenario/FTA-some_waypoints_with_time_constraint.yaml +$(find-pkg-share scenario_test_runner)/scenario/FTA-with_target_speed_and_no_time_constraint.yaml +$(find-pkg-share scenario_test_runner)/scenario/FTA-with_global_positions.yaml diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index b8ede8f6cde..8c2267bd676 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 17.5.3 + 17.6.1 scenario test runner package Tatsuya Yamasaki Apache License 2.0 diff --git a/test_runner/scenario_test_runner/scenario/FTA-only_destination_with_time_constraint.yaml b/test_runner/scenario_test_runner/scenario/FTA-only_destination_with_time_constraint.yaml new file mode 100644 index 00000000000..bc0093c7ba7 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/FTA-only_destination_with_time_constraint.yaml @@ -0,0 +1,196 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: '2025-06-23T14:13:14.940Z' + description: '' + author: Michał Ciasnocha + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34705' + s: 37.7025 + offset: -0.149 + Orientation: &ORIENTATION_ZERO + type: relative + h: 0 + p: -0.0 + r: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + Action: + - name: 'follow_trajectory' + PrivateAction: + - RoutingAction: + FollowTrajectoryAction: + initialDistanceOffset: 1 + TimeReference: + Timing: + domainAbsoluteRelative: relative + offset: 0 + scale: 1 + TrajectoryFollowingMode: + followingMode: position + TrajectoryRef: + Trajectory: + closed: false + name: straight + Shape: + Polyline: + Vertex: + - Position: + LanePosition: + roadId: '' + laneId: '34753' + s: 2.0578 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34753' + s: 6.4792 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34654' + s: 0.228 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34654' + s: 5.3399 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34654' + s: 10.4819 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34654' + s: 15.0000 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: &POSITION_EGO_DESTINATION + LanePosition: + roadId: '' + laneId: '34579' + s: 2.2747 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + time: 15.0 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *POSITION_EGO_DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + 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/FTA-some_waypoints_with_time_constraint.yaml b/test_runner/scenario_test_runner/scenario/FTA-some_waypoints_with_time_constraint.yaml new file mode 100644 index 00000000000..936a39effd5 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/FTA-some_waypoints_with_time_constraint.yaml @@ -0,0 +1,190 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: '2025-06-23T14:13:14.940Z' + description: '' + author: Michał Ciasnocha + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34705' + s: 37.7025 + offset: -0.149 + Orientation: &ORIENTATION_ZERO + type: relative + h: 0 + p: -0.0 + r: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + Action: + - name: 'follow_trajectory' + PrivateAction: + - RoutingAction: + FollowTrajectoryAction: + initialDistanceOffset: 1 + TimeReference: + Timing: + domainAbsoluteRelative: relative + offset: 0 + scale: 1 + TrajectoryFollowingMode: + followingMode: position + TrajectoryRef: + Trajectory: + closed: false + name: straight + Shape: + Polyline: + Vertex: + - Position: + LanePosition: + roadId: '' + laneId: '34753' + s: 2.0578 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + time: 5.0 + - Position: + LanePosition: + roadId: '' + laneId: '34753' + s: 6.4792 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34654' + s: 0.228 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34654' + s: 5.3399 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + time: 15.0 + - Position: + LanePosition: + roadId: '' + laneId: '34654' + s: 10.4819 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + - Position: &POSITION_EGO_DESTINATION + LanePosition: + roadId: '' + laneId: '34579' + s: 8.0 + offset: 0.0 + Orientation: *ORIENTATION_ZERO + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *POSITION_EGO_DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + 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/FTA-with_global_positions.yaml b/test_runner/scenario_test_runner/scenario/FTA-with_global_positions.yaml new file mode 100644 index 00000000000..ca07d056388 --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/FTA-with_global_positions.yaml @@ -0,0 +1,192 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: '2025-06-23T14:13:14.940Z' + description: '' + author: Michał Ciasnocha + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + WorldPosition: + x: 3722.6051 + y: 73775.6432 + z: -0.0388 + h: 0.4703 + p: -0.0 + r: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + Action: + - name: 'follow_trajectory' + PrivateAction: + - RoutingAction: + FollowTrajectoryAction: + initialDistanceOffset: 1 + TimeReference: + Timing: + domainAbsoluteRelative: relative + offset: 0 + scale: 1 + TrajectoryFollowingMode: + followingMode: position + TrajectoryRef: + Trajectory: + closed: false + name: straight + Shape: + Polyline: + Vertex: + - Position: + WorldPosition: + x: 3732.6971 + y: 73780.6796 + z: -1.3515 + h: 0.4703 + p: -0.0 + r: 0 + - Position: + WorldPosition: + x: 3741.9409 + y: 73783.6015 + z: -1.4037 + h: -0.165 + p: 0 + r: 0 + - Position: + WorldPosition: + x: 3748.4238 + y: 73779.3885 + z: -1.3027 + h: -0.9065 + p: 0 + r: 0 + - Position: + WorldPosition: + x: 3759.6645 + y: 73757.1868 + z: -0.4634 + h: -1.0919 + p: 0 + r: 0 + - Position: + WorldPosition: + x: 3762.6644 + y: 73744.0287 + z: 0.0566 + h: -2.1174 + p: 0 + r: 0 + - Position: &POSITION_EGO_DESTINATION + WorldPosition: + x: 3750.3574 + y: 73736.3256 + z: 0.3858 + h: -2.6708 + p: 0 + r: 0 + time: 12.0 + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *POSITION_EGO_DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + 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/FTA-with_target_speed_and_no_time_constraint.yaml b/test_runner/scenario_test_runner/scenario/FTA-with_target_speed_and_no_time_constraint.yaml new file mode 100644 index 00000000000..44f5fcc8cbd --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/FTA-with_target_speed_and_no_time_constraint.yaml @@ -0,0 +1,197 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: '2025-06-23T14:13:14.940Z' + description: '' + author: Michał Ciasnocha + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: [] + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 29.5076 + offset: -0.0843 + Orientation: &ORIENTATION_ZERO + type: relative + h: 0 + p: -0.0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 5 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + Action: + - name: 'follow_trajectory' + PrivateAction: + - RoutingAction: + FollowTrajectoryAction: + initialDistanceOffset: 1 + TimeReference: + Timing: + domainAbsoluteRelative: relative + offset: 0 + scale: 1 + TrajectoryFollowingMode: + followingMode: position + TrajectoryRef: + Trajectory: + closed: false + name: straight + Shape: + Polyline: + Vertex: + - Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 40.8094 + offset: -0.1684 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34498' + s: 5.253 + offset: 0.2477 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34498' + s: 12.8714 + offset: 0.6223 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34408' + s: 22.2865 + offset: -0.0543 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34645' + s: 11.5812 + offset: 0.3208 + Orientation: *ORIENTATION_ZERO + - Position: &POSITION_EGO_DESTINATION + LanePosition: + roadId: '' + laneId: '34621' + s: 6.7489 + offset: 0.2485 + Orientation: *ORIENTATION_ZERO + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *POSITION_EGO_DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: []