diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index 0182a116a09..ff6333627dc 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 5dc3f051722..d9ecdbb3fd2 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 7.4.7 + 8.0.0 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index 992b36787d5..8195139fffa 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,24 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* fix(traffic_simulator): fix follow_trajectory_action issue, add orientation to distance calc, remove toCanonicalizedLaneletPose(point...) because it can cause a another issues +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp new file mode 100644 index 00000000000..7a31f056a7f --- /dev/null +++ b/common/math/geometry/include/geometry/quaternion/direction_to_quaternion.hpp @@ -0,0 +1,46 @@ +// 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__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ +#define GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ + +#include +#include +#include +#include +#include + +namespace math +{ +namespace geometry +{ +auto convertDirectionToQuaternion(const double dx, const double dy, const double dz) +{ + const auto euler_angles = geometry_msgs::build() + .x(0.0) + .y(std::atan2(-dz, std::hypot(dx, dy))) + .z(std::atan2(dy, dx)); + return math::geometry::convertEulerAngleToQuaternion(euler_angles); +} + +template < + typename T, std::enable_if_t>, std::nullptr_t> = nullptr> +auto convertDirectionToQuaternion(const T & v) +{ + return convertDirectionToQuaternion(v.x, v.y, v.z); +} +} // namespace geometry +} // namespace math + +#endif // GEOMETRY__QUATERNION__DIRECTION_TO_QUATERNION_HPP_ diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index 564a3ef0169..bdb3dea36f6 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 7.4.7 + 8.0.0 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 c245f8a50a0..7741537708e 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index c9a676d3342..3ea2243afea 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 7.4.7 + 8.0.0 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index a16694786da..18d0ba20a8f 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index 4e2d88bc814..410a16a0a55 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 7.4.7 + 8.0.0 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 91deca738ae..251a73746e1 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index 2d588cbc5f1..e3a4ab80c37 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 7.4.7 + 8.0.0 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index c398879d1d1..c9ea28651a9 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 9e50045c142..665c5a8a469 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 7.4.7 + 8.0.0 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 29538722366..1151442fdfb 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,21 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index 973ae0abed8..6285a80ea8c 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 7.4.7 + 8.0.0 vendor packages for intel raytracing kernel library masaya Apache 2.0 diff --git a/map/kashiwanoha_map/CHANGELOG.rst b/map/kashiwanoha_map/CHANGELOG.rst index ba98ed70a75..03e4f352d6c 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 0512ece1106..844d67fa629 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 7.4.7 + 8.0.0 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 c394e67fcb3..3534ac3cacf 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,21 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index 848fef5504d..7e58863f6bb 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 7.4.7 + 8.0.0 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 79ec3bb17fb..e6610d6a1bf 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,26 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(cpp_scenario_mock): remove unused auto_sink variable +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): improve Configuration, traffic_rules, lanelet_wrapper +* feat(cpp_mock_scenarios): adapt cpp screnarios for using pose:: from lanelet_wrapper +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp index 45c2aa6962e..3b407791ef8 100644 --- a/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp +++ b/mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp @@ -75,14 +75,9 @@ class CppScenarioNode : public rclcpp::Node const std::string & scenario_filename, const bool verbose, const std::set & auto_sink_entity_types = {}) -> traffic_simulator::Configuration { - auto configuration = traffic_simulator::Configuration(map_path); - { - configuration.lanelet2_map_file = lanelet2_map_file; - // configuration.lanelet2_map_file = "lanelet2_map_with_private_road_and_walkway_ele_fix.osm"; - configuration.scenario_path = scenario_filename; - configuration.verbose = verbose; - configuration.auto_sink_entity_types = auto_sink_entity_types; - } + auto configuration = traffic_simulator::Configuration( + map_path, lanelet2_map_file, scenario_filename, auto_sink_entity_types); + configuration.verbose = verbose; checkConfiguration(configuration); return configuration; } diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index a478d74021c..9071b44eb50 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 7.4.7 + 8.0.0 C++ mock scenarios masaya Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp index d961cde5e32..80dff74fea3 100644 --- a/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp +++ b/mock/cpp_mock_scenarios/src/behavior_plugin/load_do_nothing_plugin.cpp @@ -67,27 +67,21 @@ class LoadDoNothingPluginScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters(), traffic_simulator::entity::VehicleEntity::BuiltinBehavior::doNothing()); api_.spawn( - "pedestrian", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 3.0, 0.0, api_.getHdmapUtils()), + "pedestrian", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 3.0, 0.0), getPedestrianParameters(), traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::doNothing()); api_.spawn( "vehicle_spawn_with_behavior_tree", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 2.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 2.0, 0.0), getVehicleParameters(), traffic_simulator::entity::VehicleEntity::BuiltinBehavior::behaviorTree()); api_.spawn( "pedestrian_spawn_with_behavior_tree", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 3.0, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 3.0, 0.0), getPedestrianParameters(), traffic_simulator::entity::PedestrianEntity::BuiltinBehavior::behaviorTree()); } diff --git a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp index 3ebe51a9925..4596322662f 100644 --- a/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp +++ b/mock/cpp_mock_scenarios/src/collision/crashing_npc.cpp @@ -52,9 +52,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 15, true); @@ -63,9 +61,7 @@ class CrashingNpcScenario : public cpp_mock_scenarios::CppScenarioNode api_.setBehaviorParameter("ego", behavior_parameter); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 0.0); api_.requestSpeedChange("npc", 5, true); diff --git a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp index 75f19793f70..522145b3de7 100644 --- a/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/collision/spawn_with_offset.cpp @@ -50,17 +50,13 @@ class SpawnWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.2, 1.3, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.2, 1.3), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange("ego", 0, true); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, -0.874, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, -0.874), getPedestrianParameters()); api_.setLinearVelocity("bob", 0); api_.requestSpeedChange("bob", 0, true); diff --git a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp index 4b94b5ee5d2..0072527287c 100644 --- a/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp +++ b/mock/cpp_mock_scenarios/src/crosswalk/stop_at_crosswalk.cpp @@ -74,23 +74,17 @@ class StopAtCrosswalkScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0.0)}); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34378, 0.0, 0.0, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0.0), getPedestrianParameters()); api_.setLinearVelocity("bob", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp index 266080f9961..4ee77de7bc8 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/accelerate_and_follow.cpp @@ -59,16 +59,12 @@ class AccelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 10); api_.requestSpeedChange("npc", 10, true); diff --git a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp index e8bbcda8abe..c6b833663d4 100644 --- a/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp +++ b/mock/cpp_mock_scenarios/src/follow_front_entity/decelerate_and_follow.cpp @@ -59,16 +59,12 @@ class DecelerateAndFollowScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 15); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 15.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 15.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 10); api_.requestSpeedChange("npc", 10, true); diff --git a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp index 450de807a29..01463611f9e 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/acquire_position_in_world_frame.cpp @@ -47,19 +47,14 @@ class AcquirePositionInWorldFrameScenario : public cpp_mock_scenarios::CppScenar void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setEntityStatus( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), traffic_simulator::helper::constructActionStatus(10)); api_.requestSpeedChange("ego", 10, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34408, 1.0, 0.0, api_.getHdmapUtils())); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 1.0, 0.0)); api_.requestAcquirePosition("ego", goal_pose); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp index 9794dc48934..8df5d2906e4 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/assign_route_in_world_frame.cpp @@ -47,19 +47,15 @@ class AcquireRouteInWorldFrameScenario : public cpp_mock_scenarios::CppScenarioN void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); std::vector goal_poses; goal_poses.emplace_back(traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34408, 1.0, 0.0, api_.getHdmapUtils()))); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 1.0, 0.0))); goal_poses.emplace_back(traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34408, 10, 0.0, api_.getHdmapUtils()))); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34408, 10, 0.0))); api_.requestAssignRoute("ego", goal_poses); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp index 8db02b87da9..cc09337e277 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/cancel_request.cpp @@ -40,10 +40,7 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { if (api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 30, 0, api_.getHdmapUtils()), - 3.0)) { + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 30, 0), 3.0)) { api_.cancelRequest("ego"); canceled = true; } @@ -54,14 +51,12 @@ class CancelRequestScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 7); api_.requestSpeedChange("ego", 7, true); const geometry_msgs::msg::Pose goal_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0), api_.getHdmapUtils()); + traffic_simulator::helper::constructLaneletPose(34408, 0.0, 0.0)); api_.requestAcquirePosition("ego", goal_pose); } }; diff --git a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp index c3f2c16306d..0c165176e9d 100644 --- a/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp +++ b/mock/cpp_mock_scenarios/src/follow_lane/follow_with_offset.cpp @@ -55,9 +55,7 @@ class FollowLaneWithOffsetScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 3.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 3.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp index 9dfe60ac151..0a0a8ee6bd4 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left.cpp @@ -51,9 +51,7 @@ class LaneChangeLeftScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp index c04184811e8..5745bd5eb82 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_left_with_id.cpp @@ -51,9 +51,7 @@ class LaneChangeLeftWithIdScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp index 6cabbd683a8..face1ea094a 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear.cpp @@ -55,9 +55,7 @@ class LaneChangeLinearScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp index f6bf51b7c80..669f38f0640 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_lateral_velocity.cpp @@ -59,9 +59,7 @@ class LaneChangeLinearLateralVelocityScenario : public cpp_mock_scenarios::CppSc void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp index bc00c1b7640..91eb620c7db 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_linear_time.cpp @@ -60,9 +60,7 @@ class LaneChangeLinearTimeScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp index e04070e2380..119c6672db2 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_longitudinal_distance.cpp @@ -47,9 +47,7 @@ class LaneChangeLongitudinalDistanceScenario : public cpp_mock_scenarios::CppSce void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 1); api_.requestSpeedChange("ego", 1, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp index 0210fc3c154..d0d5c98edc4 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right.cpp @@ -51,9 +51,7 @@ class LaneChangeRightScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp index eb3e2690d9e..a5566f47f44 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_right_with_id.cpp @@ -51,9 +51,7 @@ class LaneChangeRightWithIdScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp index 88a2c446d61..1f07591bf70 100644 --- a/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp +++ b/mock/cpp_mock_scenarios/src/lane_change/lanechange_time.cpp @@ -60,9 +60,7 @@ class LaneChangeTimeScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 10, true); diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp index 11e07f35501..915b508b0dc 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp @@ -144,25 +144,19 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 5.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 5.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 3, true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 10.0, 1.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 10.0, 1.0), getVehicleParameters()); api_.setLinearVelocity("front", 10); api_.requestSpeedChange("front", 3, true); api_.spawn( - "behind", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, -1.0, api_.getHdmapUtils()), + "behind", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, -1.0), getVehicleParameters()); api_.setLinearVelocity("behind", 10); api_.requestSpeedChange("behind", 3, true); diff --git a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp index 4003c78f731..b6731a63ae8 100644 --- a/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp +++ b/mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp @@ -58,9 +58,7 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 5.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 5.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 3, true); diff --git a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp index 44978b272e3..cf20ad99b84 100644 --- a/mock/cpp_mock_scenarios/src/merge/merge_left.cpp +++ b/mock/cpp_mock_scenarios/src/merge/merge_left.cpp @@ -54,18 +54,14 @@ class MergeLeftScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34462, 15.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34462, 15.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 5); api_.requestSpeedChange("ego", 5, true); api_.requestLaneChange("ego", 34513); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc", 10); } diff --git a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp index ee4786cc94d..9e842315754 100644 --- a/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp +++ b/mock/cpp_mock_scenarios/src/metrics/traveled_distance.cpp @@ -68,9 +68,7 @@ class TraveledDistanceScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3, true); diff --git a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp index 08cb36d12f6..eee09676089 100644 --- a/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp +++ b/mock/cpp_mock_scenarios/src/move_backward/move_backward.cpp @@ -52,9 +52,7 @@ class MoveBackwardScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", -3); api_.requestSpeedChange("ego", -3, true); diff --git a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp index 6f57fb4e666..82a53ad57ec 100644 --- a/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp +++ b/mock/cpp_mock_scenarios/src/pedestrian/walk_straight.cpp @@ -70,22 +70,16 @@ class WalkStraightScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0.0)}); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34378, 0.0, 0.0, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0.0), getPedestrianParameters()); api_.setLinearVelocity("bob", 0); api_.requestWalkStraight("bob"); diff --git a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp index 0a6a6e4b11e..9e5019df4f7 100644 --- a/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp +++ b/mock/cpp_mock_scenarios/src/random_scenario/random001.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -70,9 +71,9 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode traffic_simulator::helper::constructCanonicalizedLaneletPose( spawn_lanelet_id, static_cast(entity_index) / static_cast(number_of_vehicles) * - traffic_simulator::pose::laneletLength(spawn_lanelet_id, api_.getHdmapUtils()) + + traffic_simulator::lanelet_map::laneletLength(spawn_lanelet_id) + normal_dist(engine_), - offset, api_.getHdmapUtils()), + offset), getVehicleParameters( get_entity_subtype(params_.random_parameters.road_parking_vehicle.entity_type))); api_.requestSpeedChange(entity_name, 0, true); @@ -122,8 +123,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode if (!api_.entityExists(entity_name)) { api_.spawn( entity_name, - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, spawn_s_value, 0.0, api_.getHdmapUtils()), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, spawn_s_value, 0.0), getVehicleParameters()); std::uniform_real_distribution<> speed_distribution( params_.random_parameters.lane_following_vehicle.min_speed, @@ -132,7 +132,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode api_.requestSpeedChange(entity_name, speed, true); api_.setLinearVelocity(entity_name, speed); std::uniform_real_distribution<> lane_change_position_distribution( - 0.0, traffic_simulator::pose::laneletLength(34684, api_.getHdmapUtils())); + 0.0, traffic_simulator::lanelet_map::laneletLength(34684)); lane_change_position = lane_change_position_distribution(engine_); lane_change_requested = false; } @@ -157,9 +157,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode if ( !api_.entityExists(entity_name) && !api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34576, 25.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34576, 25.0, 0.0), 5.0)) { std::normal_distribution<> offset_distribution( 0.0, params_.random_parameters.crossing_pedestrian.offset_variance); @@ -169,7 +167,7 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode api_.spawn( entity_name, traffic_simulator::helper::constructCanonicalizedLaneletPose( - lanelet_id, 0.0, offset_distribution(engine_), api_.getHdmapUtils()), + lanelet_id, 0.0, offset_distribution(engine_)), getPedestrianParameters()); const auto speed = speed_distribution(engine_); api_.requestSpeedChange(entity_name, speed, true); @@ -184,10 +182,10 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode } { - const auto trigger_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34621, 10, 0.0, api_.getHdmapUtils()); - const auto ego_goal_position = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 0.0, 0.0, api_.getHdmapUtils()); + const auto trigger_position = + traffic_simulator::helper::constructCanonicalizedLaneletPose(34621, 10, 0.0); + const auto ego_goal_position = + traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 0.0, 0.0); const auto entity_name = "spawn_nearby_ego"; if (const auto ego = api_.getEntity("ego")) { @@ -223,10 +221,8 @@ class RandomScenario : public cpp_mock_scenarios::CppScenarioNode spawnRoadParkingVehicles(); spawnEgoEntity( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34621, 10.0, 0.0, api_.getHdmapUtils()), - {traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 0.0, 0.0, api_.getHdmapUtils())}, + traffic_simulator::helper::constructCanonicalizedLaneletPose(34621, 10.0, 0.0), + {traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 0.0, 0.0)}, getVehicleParameters()); if (const auto ego = api_.getEntity("ego")) { api_.spawn( diff --git a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp index f1db7fd4e06..7e309765df4 100644 --- a/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp +++ b/mock/cpp_mock_scenarios/src/respawn_ego/respawn_ego.cpp @@ -30,8 +30,7 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode : cpp_mock_scenarios::CppScenarioNode( "respawn_ego", ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map", "lanelet2_map.osm", __FILE__, false, option), - goal_pose{traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 0, 0, api_.getHdmapUtils())}, + goal_pose{traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 0, 0)}, new_position_subscriber{create_subscription( "/initialpose", rclcpp::QoS(rclcpp::KeepLast(1)).reliable(), [this](const geometry_msgs::msg::PoseWithCovarianceStamped & message) { @@ -55,9 +54,8 @@ class RespawnEgoScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { spawnEgoEntity( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34621, 10.0, 0.0, api_.getHdmapUtils()), - {goal_pose}, getVehicleParameters()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34621, 10.0, 0.0), {goal_pose}, + getVehicleParameters()); } private: diff --git a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp index 56d48813a89..e31df21eef0 100644 --- a/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp +++ b/mock/cpp_mock_scenarios/src/spawn/spawn_in_map_frame.cpp @@ -39,8 +39,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode void onUpdate() override { const auto map_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())); + traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0)); if (api_.reachPosition("ego", map_pose, 0.1)) { stop(cpp_mock_scenarios::Result::SUCCESS); } else { @@ -53,8 +52,7 @@ class SpawnInMapFrameScenario : public cpp_mock_scenarios::CppScenarioNode api_.spawn( "ego", traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils())), + traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0)), getVehicleParameters()); } }; diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp index 548794166b8..9f00065436a 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change.cpp @@ -55,9 +55,7 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( @@ -67,9 +65,7 @@ class RequestSpeedChangeScenario : public cpp_mock_scenarios::CppScenarioNode true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp index 372ba2c0658..a83a987e2f8 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_continuous_false.cpp @@ -62,9 +62,7 @@ class RequestSpeedChangeContinuousFalseScenario : public cpp_mock_scenarios::Cpp { speed_reached = false; api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp index 7d277e44b5c..f65f1f28e83 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_relative.cpp @@ -76,17 +76,13 @@ class RequestSpeedChangeRelativeScenario : public cpp_mock_scenarios::CppScenari void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3.0, true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 3); } diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp index b3c672a6627..a9d6755a600 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_step.cpp @@ -52,9 +52,7 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( @@ -64,9 +62,7 @@ class RequestSpeedChangeStepScenario : public cpp_mock_scenarios::CppScenarioNod true); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp index 25afc545654..d63a10f746f 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_limit.cpp @@ -52,9 +52,7 @@ class RequestSpeedChangeWithLimitScenario : public cpp_mock_scenarios::CppScenar void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.setVelocityLimit("ego", 5.0); diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp index 7c0b4d11b48..c3b47049b38 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint.cpp @@ -56,9 +56,7 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( @@ -68,9 +66,7 @@ class RequestSpeedChangeWithTimeConstraintScenario : public cpp_mock_scenarios:: false); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 10); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp index 735ab50a3bc..a077508ba0f 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_linear.cpp @@ -52,9 +52,7 @@ class RequestSpeedChangeWithTimeConstraintLinearScenario void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp index 17992f4d687..ba8e3eceee9 100644 --- a/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp +++ b/mock/cpp_mock_scenarios/src/speed_planning/request_speed_change_with_time_constraint_relative.cpp @@ -57,9 +57,7 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 1.0); api_.requestSpeedChange( @@ -72,9 +70,7 @@ class RequestSpeedChangeWithTimeConstraintRelativeScenario false); api_.spawn( - "front", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34741, 10.0, 0.0, api_.getHdmapUtils()), + "front", traffic_simulator::helper::constructCanonicalizedLaneletPose(34741, 10.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("front", 10); api_.requestSpeedChange( diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp index 42140bbee4c..f55fb1a6ae5 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action.cpp @@ -38,9 +38,9 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode private: const traffic_simulator::CanonicalizedLaneletPose ego_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0); const traffic_simulator::CanonicalizedLaneletPose npc_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0); void onUpdate() override { @@ -63,26 +63,22 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34976, 20, 0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34976, 20, 0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3, true); std::vector goal_poses; - goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 20, 0, api_.getHdmapUtils())); + goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 20, 0)); api_.requestAssignRoute("ego", goal_poses); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34576, 0, 0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34576, 0, 0), getVehicleParameters()); std::vector npc_goal_poses; - npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34564, 20, 0, api_.getHdmapUtils())); + npc_goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34564, 20, 0)); api_.requestAssignRoute("npc", npc_goal_poses); api_.setLinearVelocity("npc", 6); } @@ -90,7 +86,7 @@ class SynchronizedAction : public cpp_mock_scenarios::CppScenarioNode auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) -> std::optional { - return traffic_simulator::pose::canonicalize(lanelet_pose, api_.getHdmapUtils()); + return traffic_simulator::pose::toCanonicalizedLaneletPose(lanelet_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp index 2aa571173fa..30c62fadda5 100644 --- a/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp +++ b/mock/cpp_mock_scenarios/src/synchronized_action/synchronized_action_with_speed.cpp @@ -38,9 +38,9 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode private: const traffic_simulator::CanonicalizedLaneletPose ego_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34585, 0, 0); const traffic_simulator::CanonicalizedLaneletPose npc_target = - traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0, api_.getHdmapUtils()); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0, 0); void onUpdate() override { @@ -63,26 +63,22 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34976, 20, 0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34976, 20, 0), getVehicleParameters()); api_.setLinearVelocity("ego", 3); api_.requestSpeedChange("ego", 3, true); std::vector goal_poses; - goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 20, 0, api_.getHdmapUtils())); + goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 20, 0)); api_.requestAssignRoute("ego", goal_poses); api_.spawn( - "npc", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34576, 0, 0, api_.getHdmapUtils()), + "npc", traffic_simulator::helper::constructCanonicalizedLaneletPose(34576, 0, 0), getVehicleParameters()); std::vector npc_goal_poses; - npc_goal_poses.emplace_back(traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34564, 20, 0, api_.getHdmapUtils())); + npc_goal_poses.emplace_back( + traffic_simulator::helper::constructCanonicalizedLaneletPose(34564, 20, 0)); api_.requestAssignRoute("npc", npc_goal_poses); api_.setLinearVelocity("npc", 6); } @@ -90,7 +86,7 @@ class SynchronizedActionWithSpeed : public cpp_mock_scenarios::CppScenarioNode auto getSampleLaneletPose(const traffic_simulator::LaneletPose & lanelet_pose) -> std::optional { - return traffic_simulator::pose::canonicalize(lanelet_pose, api_.getHdmapUtils()); + return traffic_simulator::pose::toCanonicalizedLaneletPose(lanelet_pose); } }; } // namespace cpp_mock_scenarios diff --git a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp index 6845404af94..ce6ed50fe33 100644 --- a/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_simulation_demo.cpp @@ -47,46 +47,35 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode } if (api_.getCurrentTime() >= 4 && api_.entityExists("obstacle")) { api_.setEntityStatus( - "obstacle", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "obstacle", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), traffic_simulator::helper::constructActionStatus(10)); } if (api_.getCurrentTime() >= 6 && api_.entityExists("obstacle")) { api_.despawn("obstacle"); } if (api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34615, 10.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34615, 10.0, 0.0), 5)) { api_.requestAcquirePosition( - "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 35026, 0.0, 0.0, api_.getHdmapUtils())); + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(35026, 0.0, 0.0)); if (api_.entityExists("npc2")) { api_.requestSpeedChange("npc2", 13, true); } } if (api_.reachPosition( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 0.0, 0.0), 5)) { api_.requestAcquirePosition( - "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils())); + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0)); if (api_.entityExists("npc2")) { api_.requestSpeedChange("npc2", 3, true); } } if (api_.reachPosition( - "npc2", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34513, 0.0, 0.0, api_.getHdmapUtils()), + "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose(34513, 0.0, 0.0), 5)) { api_.requestAcquirePosition( - "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34630, 0.0, 0.0, api_.getHdmapUtils())); + "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose(34630, 0.0, 0.0)); api_.requestSpeedChange("npc2", 13, true); } if (api_.getCurrentTime() > 10.0 && api_.entityExists("bob")) { @@ -99,18 +88,14 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode lanechange_executed_ = false; api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 120545, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(120545, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 10); api_.requestSpeedChange("ego", 8, true); api_.requestAssignRoute( "ego", std::vector{ - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils()), - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34690, 0.0, 0.0, api_.getHdmapUtils())}); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0), + traffic_simulator::helper::constructCanonicalizedLaneletPose(34690, 0.0, 0.0)}); api_.spawn( "tom", traffic_simulator::helper::constructPose(10, 3, 0, 0, 0, -1.57), @@ -122,36 +107,27 @@ class TrafficSimulationDemoScenario : public cpp_mock_scenarios::CppScenarioNode api_.requestSpeedChange("tom", 3, true); api_.spawn( - "bob", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34378, 0.0, 0.0, api_.getHdmapUtils()), + "bob", traffic_simulator::helper::constructCanonicalizedLaneletPose(34378, 0.0, 0.0), getPedestrianParameters()); api_.setLinearVelocity("bob", 1.0); api_.requestSpeedChange("bob", 1, true); api_.spawn( - "npc1", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34579, 20.0, 0.0, api_.getHdmapUtils()), + "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose(34579, 20.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc1", 5.0); api_.requestSpeedChange("npc1", 5, true); api_.requestAcquirePosition( - "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34675, 0.0, 0.0, api_.getHdmapUtils())); + "npc1", traffic_simulator::helper::constructCanonicalizedLaneletPose(34675, 0.0, 0.0)); api_.spawn( - "npc2", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34606, 20.0, 0.0, api_.getHdmapUtils()), + "npc2", traffic_simulator::helper::constructCanonicalizedLaneletPose(34606, 20.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc2", 5); api_.requestSpeedChange("npc2", 0, true); api_.spawn( - "npc3", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34468, 0.0, 0.0, api_.getHdmapUtils()), + "npc3", traffic_simulator::helper::constructCanonicalizedLaneletPose(34468, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("npc3", 10); diff --git a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp index 8d3a89c2ae7..66743054cba 100644 --- a/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_sink/auto_sink_vehicle.cpp @@ -51,8 +51,7 @@ class AutoSinkVehicleScenario : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { const auto map_pose = traffic_simulator::pose::toMapPose( - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34774, 11.0, 0.0, api_.getHdmapUtils())); + traffic_simulator::helper::constructCanonicalizedLaneletPose(34774, 11.0, 0.0)); api_.spawn("car", map_pose, getVehicleParameters()); api_.spawn("bob", map_pose, getPedestrianParameters()); } diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp index 46532152560..739e39b2c45 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_delay.cpp @@ -92,9 +92,7 @@ class DefineTrafficSourceDelay : public cpp_mock_scenarios::CppScenarioNode void onInitialize() override { api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp index 7a5c8d97a83..7b5e818196c 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_high_rate.cpp @@ -82,9 +82,7 @@ class DefineTrafficSourceHighRate : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp index 6d7976cfa05..31b1b884f3d 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_large.cpp @@ -78,9 +78,7 @@ class DefineTrafficSourceLarge : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp index 4f9141d9d92..8eb2927d3bd 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_mixed.cpp @@ -93,9 +93,7 @@ class DefineTrafficSourceMixed : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp index f4ba78518ff..685637fbac0 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_multiple.cpp @@ -115,9 +115,7 @@ class DefineTrafficSourceMultiple : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp index 02db2b0bd26..d7d924387a1 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_outside_lane.cpp @@ -79,9 +79,7 @@ class DefineTrafficSourceOutsideLane : public cpp_mock_scenarios::CppScenarioNod true, false, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp index 02c46bbd755..7f05f3d80a9 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_pedestrian.cpp @@ -82,9 +82,7 @@ class DefineTrafficSourcePedestrian : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp index d38b251004f..72c2e146c37 100644 --- a/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp +++ b/mock/cpp_mock_scenarios/src/traffic_source/define_traffic_source_vehicle.cpp @@ -82,9 +82,7 @@ class DefineTrafficSourceVehicle : public cpp_mock_scenarios::CppScenarioNode false, true, true, 0); api_.spawn( - "ego", - traffic_simulator::helper::constructCanonicalizedLaneletPose( - 34570, 0.0, 0.0, api_.getHdmapUtils()), + "ego", traffic_simulator::helper::constructCanonicalizedLaneletPose(34570, 0.0, 0.0), getVehicleParameters()); api_.setLinearVelocity("ego", 0.0); api_.requestSpeedChange("ego", 0.0, true); diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 177be56afb6..39726efd4f7 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index c53e2541c23..f0abd71b19c 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 7.4.7 + 8.0.0 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 9a78f393758..9b7ce642566 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,25 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): improve Configuration, traffic_rules, lanelet_wrapper +* feat(traffic_simulator, openscenario_interpreter, behavior_tree_plugin, simple_sensor_simulator): use pose:: from lanelet_wrapper instead of hdmap_utils methods +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 5252176abfc..aace1bfc6ba 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -74,7 +74,7 @@ class SimulatorCore static auto canonicalize(const traffic_simulator::LaneletPose & non_canonicalized) -> NativeLanePosition { - return NativeLanePosition(non_canonicalized, core->getHdmapUtils()); + return NativeLanePosition(non_canonicalized); } template , int> = 0> @@ -82,8 +82,8 @@ class SimulatorCore { constexpr bool include_crosswalk{false}; if ( - const auto result = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, include_crosswalk, core->getHdmapUtils())) { + const auto result = + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, include_crosswalk)) { return result.value(); } else { throw Error( diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 9c3006ab4a8..2ad54154a77 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 7.4.7 + 8.0.0 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index dae7d8fd8df..74fdf7ca028 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -71,19 +71,18 @@ auto Interpreter::currentScenarioDefinition() const -> const std::shared_ptr traffic_simulator::Configuration { const auto logic_file = currentScenarioDefinition()->road_network.logic_file; - - auto configuration = traffic_simulator::Configuration( - logic_file.isDirectory() ? logic_file : logic_file.filepath.parent_path()); - { - configuration.scenario_path = osc_path; - - // XXX DIRTY HACK!!! - if (not logic_file.isDirectory() and logic_file.filepath.extension() == ".osm") { - configuration.lanelet2_map_file = logic_file.filepath.filename().string(); - } + const auto is_directly_lanelet2_map_file = + not logic_file.isDirectory() and logic_file.filepath.extension() == ".osm"; + const auto map_files_path = + is_directly_lanelet2_map_file ? logic_file.filepath.parent_path() : logic_file; + + // XXX DIRTY HACK!!! + if (is_directly_lanelet2_map_file) { + return traffic_simulator::Configuration( + map_files_path, logic_file.filepath.filename().string(), osc_path); + } else { + return traffic_simulator::Configuration(map_files_path, osc_path); } - - return configuration; } auto Interpreter::on_configure(const rclcpp_lifecycle::State &) -> Result diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index 9ea19ba94cf..905a2233fd5 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index cecd544df67..18a55f7dc45 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 7.4.7 + 8.0.0 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index ac9f3ba6620..11273a031d1 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 11ee020473c..57c83eec83d 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 7.4.7 + 8.0.0 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 719cf15cd9c..a355fa7bd2b 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index 547605bd508..3f746f2ddc8 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 7.4.7 + 8.0.0 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 99ec1e7f60f..ae6a91a1e7a 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index b9e101ae325..cf519748af8 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 7.4.7 + 8.0.0 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 4748b10e56c..503dfa91372 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,21 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index ff481f0df10..ead8a18caaa 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 7.4.7 + 8.0.0 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 1ac922132bd..8e38a62c983 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,21 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index 3d10a7647d7..a089b897e57 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 7.4.7 + 8.0.0 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 dbd9052b85d..efb1bc82773 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index 36a541ae9af..4032b16dd46 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 7.4.7 + 8.0.0 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 c172007450e..78c38a5ce12 100644 --- a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst +++ b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix 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 8849a6a2cb0..8c6d01ab007 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 - 7.4.7 + 8.0.0 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 d0b5334b048..a6ccb639042 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 959875c6c80..647add6e059 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 7.4.7 + 8.0.0 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 1ecc369ad0b..48831097221 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,28 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): refactor laneletAltitude and isAltitudeMatching after merge +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): apply sonar recommendations to lanelet_wrapper and utils +* feat(traffic_simulator, behavior_tree_plugin): use lanelet_wrapper::lanelet_map in parts previusly overlooked +* feat(traffic_simulator, behavior_tree_plugin): use lanelet_wrapper::pose in parts previously overlooked +* feat(traffic_simulator, openscenario_interpreter, behavior_tree_plugin, simple_sensor_simulator): use pose:: from lanelet_wrapper instead of hdmap_utils methods +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge pull request `#1507 `_ from tier4/RJD-1511/bug_fix diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp index dc5d2f1a8e1..3c71568a435 100644 --- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp +++ b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -106,10 +107,10 @@ class ActionNode : public BT::ActionNodeBase auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) -> void; auto calculateUpdatedEntityStatus( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const + const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus; auto calculateUpdatedEntityStatusInWorldFrame( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const + const double local_target_speed, const traffic_simulator_msgs::msg::DynamicConstraints &) const -> traffic_simulator::EntityStatus; protected: diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 9e9a0f6195b..1b5fe00b92f 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 7.4.7 + 8.0.0 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 83374ca08e6..c7980bc2063 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -106,7 +107,7 @@ auto ActionNode::setCanonicalizedEntityStatus(const traffic_simulator::EntitySta -> void { canonicalized_entity_status->set( - entity_status, default_matching_distance_for_lanelet_pose_calculation, hdmap_utils); + entity_status, default_matching_distance_for_lanelet_pose_calculation); } auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const @@ -302,13 +303,9 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_right, double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - const auto & status = getEntityStatus(target_name); - if (status.laneMatchingSucceed()) { - return getDistanceToTargetEntityPolygon( - spline, status, width_extension_right, width_extension_left, length_extension_front, - length_extension_rear); - } - return std::nullopt; + return getDistanceToTargetEntityPolygon( + spline, getEntityStatus(target_name), width_extension_right, width_extension_left, + length_extension_front, length_extension_rear); } auto ActionNode::getDistanceToTargetEntityPolygon( @@ -317,21 +314,27 @@ auto ActionNode::getDistanceToTargetEntityPolygon( double width_extension_left, double length_extension_front, double length_extension_rear) const -> std::optional { - if (status.laneMatchingSucceed() && isOtherEntityAtConsideredAltitude(status)) { + if (isOtherEntityAtConsideredAltitude(status)) { const auto polygon = math::geometry::transformPoints( status.getMapPose(), math::geometry::getPointsFromBbox( status.getBoundingBox(), width_extension_right, width_extension_left, length_extension_front, length_extension_rear)); return spline.getCollisionPointIn2D(polygon, false); + } else { + return std::nullopt; } - return std::nullopt; } auto ActionNode::isOtherEntityAtConsideredAltitude( const traffic_simulator::CanonicalizedEntityStatus & entity_status) const -> bool { - return hdmap_utils->isAltitudeMatching( - canonicalized_entity_status->getAltitude(), entity_status.getAltitude()); + if (canonicalized_entity_status->laneMatchingSucceed() && entity_status.laneMatchingSucceed()) { + return traffic_simulator::pose::isAltitudeMatching( + canonicalized_entity_status->getCanonicalizedLaneletPose().value(), + entity_status.getCanonicalizedLaneletPose().value()); + } else { + return false; + } } auto ActionNode::getDistanceToConflictingEntity( @@ -415,100 +418,49 @@ auto ActionNode::foundConflictingEntity(const lanelet::Ids & following_lanelets) } auto ActionNode::calculateUpdatedEntityStatus( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const + const double local_target_speed, + const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> traffic_simulator::EntityStatus { const auto speed_planner = traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, canonicalized_entity_status->getTwist(), + local_target_speed, constraints, canonicalized_entity_status->getTwist(), canonicalized_entity_status->getAccel()); const double linear_jerk_new = std::get<2>(dynamics); const geometry_msgs::msg::Accel accel_new = std::get<1>(dynamics); const geometry_msgs::msg::Twist twist_new = std::get<0>(dynamics); - - if (!canonicalized_entity_status->laneMatchingSucceed()) { - THROW_SIMULATION_ERROR( - "Entity ", canonicalized_entity_status->getName(), " is not matched to the lanelet."); - } else { - auto lanelet_pose = canonicalized_entity_status->getLaneletPose(); - lanelet_pose.s = - lanelet_pose.s + + if ( + const auto canonicalized_lanelet_pose = + canonicalized_entity_status->getCanonicalizedLaneletPose()) { + const auto distance = (twist_new.linear.x + canonicalized_entity_status->getTwist().linear.x) / 2.0 * step_time; - const auto canonicalized = hdmap_utils->canonicalizeLaneletPose(lanelet_pose, route_lanelets); - if ( - const auto canonicalized_lanelet_pose = - std::get>(canonicalized)) { - // If canonicalize succeed, set canonicalized pose and set other values. - auto entity_status_updated = - static_cast(*canonicalized_entity_status); - { - entity_status_updated.time = current_time + step_time; - entity_status_updated.lanelet_pose = canonicalized_lanelet_pose.value(); - entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.action_status.twist = twist_new; - entity_status_updated.action_status.accel = accel_new; - entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.pose = - hdmap_utils->toMapPose(canonicalized_lanelet_pose.value()).pose; - } - return entity_status_updated; - } else { - // If canonicalize failed, set end of road lanelet pose. - if (const auto end_of_road_lanelet_id = std::get>(canonicalized)) { - if (lanelet_pose.s < 0) { - traffic_simulator::LaneletPose end_of_road_lanelet_pose; - { - end_of_road_lanelet_pose.lanelet_id = end_of_road_lanelet_id.value(); - end_of_road_lanelet_pose.s = 0; - end_of_road_lanelet_pose.offset = lanelet_pose.offset; - end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; - } - auto entity_status_updated = - static_cast(*canonicalized_entity_status); - { - entity_status_updated.time = current_time + step_time; - entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; - entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.action_status.twist = twist_new; - entity_status_updated.action_status.accel = accel_new; - entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; - } - return entity_status_updated; - } else { - traffic_simulator::LaneletPose end_of_road_lanelet_pose; - { - end_of_road_lanelet_pose.lanelet_id = end_of_road_lanelet_id.value(); - end_of_road_lanelet_pose.s = - hdmap_utils->getLaneletLength(end_of_road_lanelet_id.value()); - end_of_road_lanelet_pose.offset = lanelet_pose.offset; - end_of_road_lanelet_pose.rpy = lanelet_pose.rpy; - } - auto entity_status_updated = - static_cast(*canonicalized_entity_status); - { - entity_status_updated.time = current_time + step_time; - entity_status_updated.lanelet_pose = end_of_road_lanelet_pose; - entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.action_status.twist = twist_new; - entity_status_updated.action_status.accel = accel_new; - entity_status_updated.action_status.linear_jerk = linear_jerk_new; - entity_status_updated.pose = hdmap_utils->toMapPose(end_of_road_lanelet_pose).pose; - } - return entity_status_updated; - } - } else { - THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id."); - } - } + auto entity_status_updated = + static_cast(*canonicalized_entity_status); + entity_status_updated.time = current_time + step_time; + entity_status_updated.action_status.twist = twist_new; + entity_status_updated.action_status.accel = accel_new; + entity_status_updated.action_status.linear_jerk = linear_jerk_new; + /// @todo it will be moved to route::moveAlongLaneletPose(...) + entity_status_updated.lanelet_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + static_cast(canonicalized_lanelet_pose.value()), + route_lanelets, distance); + entity_status_updated.lanelet_pose_valid = true; + entity_status_updated.pose = + traffic_simulator::pose::toMapPose(entity_status_updated.lanelet_pose); + return entity_status_updated; + } else { + THROW_SIMULATION_ERROR( + "Cannot move along lanelet - entity ", std::quoted(canonicalized_entity_status->getName()), + " has invalid lanelet pose."); } } auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( - double target_speed, const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const + const double local_target_speed, + const traffic_simulator_msgs::msg::DynamicConstraints & constraints) const -> traffic_simulator::EntityStatus { using math::geometry::operator*; @@ -575,7 +527,7 @@ auto ActionNode::calculateUpdatedEntityStatusInWorldFrame( traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner( step_time, canonicalized_entity_status->getName()); const auto dynamics = speed_planner.getDynamicStates( - target_speed, constraints, canonicalized_entity_status->getTwist(), + local_target_speed, constraints, canonicalized_entity_status->getTwist(), canonicalized_entity_status->getAccel()); const auto linear_jerk_new = std::get<2>(dynamics); const auto & accel_new = std::get<1>(dynamics); diff --git a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp index cbf2f304c22..1443370e8a0 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/pedestrian_action_node.cpp @@ -18,7 +18,6 @@ #include #include #include -#include namespace entity_behavior { @@ -58,7 +57,7 @@ auto PedestrianActionNode::calculateUpdatedEntityStatusInWorldFrame(double targe traffic_simulator::pose::pedestrian::transformToCanonicalizedLaneletPose( entity_status_updated.pose, canonicalized_entity_status->getBoundingBox(), canonicalized_entity_status->getLaneletIds(), true, - default_matching_distance_for_lanelet_pose_calculation, hdmap_utils)) { + default_matching_distance_for_lanelet_pose_calculation)) { entity_status_updated.lanelet_pose_valid = true; entity_status_updated.lanelet_pose = static_cast(canonicalized_lanelet_pose.value()); diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp index d1412b22029..dbd4de401d4 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/move_backward_action.cpp @@ -51,7 +51,7 @@ const traffic_simulator_msgs::msg::WaypointsArray MoveBackwardAction::calculateW s_in_spline = s_in_spline + lanelet_pose.s; break; } else { - s_in_spline = hdmap_utils->getLaneletLength(id) + s_in_spline; + s_in_spline = traffic_simulator::lanelet_map::laneletLength(id) + s_in_spline; } } traffic_simulator_msgs::msg::WaypointsArray waypoints; diff --git a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp index 593915b94b0..0e6cc886761 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/lane_change_action.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include namespace entity_behavior @@ -112,6 +113,7 @@ BT::NodeStatus LaneChangeAction::tick() } std::optional> traj_with_goal; traffic_simulator::LaneletPose along_pose, goal_pose; + /// @todo traffic_simulator::lanelet_wrapper::pose::alongLaneletPose(...) will be moved to traffic_simulator::route::moveAlongLaneletPose(...) switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: /** @@ -121,27 +123,27 @@ BT::NodeStatus LaneChangeAction::tick() 1.0 is a forward_distance_threshold (If the goal x position in the cartesian coordinate was under 1.0, the goal was rejected.) */ traj_with_goal = hdmap_utils->getLaneChangeTrajectory( - hdmap_utils->toMapPose(lanelet_pose).pose, lane_change_parameters_.value(), 10.0, 20.0, - 1.0); - along_pose = hdmap_utils->getAlongLaneletPose( + traffic_simulator::pose::toMapPose(lanelet_pose), lane_change_parameters_.value(), 10.0, + 20.0, 1.0); + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, traffic_simulator::lane_change::Parameter::default_lanechange_distance); break; case traffic_simulator::lane_change::Constraint::Type::LATERAL_VELOCITY: traj_with_goal = hdmap_utils->getLaneChangeTrajectory(lanelet_pose, lane_change_parameters_.value()); - along_pose = hdmap_utils->getAlongLaneletPose( + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, traffic_simulator::lane_change::Parameter::default_lanechange_distance); break; case traffic_simulator::lane_change::Constraint::Type::LONGITUDINAL_DISTANCE: traj_with_goal = hdmap_utils->getLaneChangeTrajectory(lanelet_pose, lane_change_parameters_.value()); - along_pose = hdmap_utils->getAlongLaneletPose( + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, lane_change_parameters_->constraint.value); break; case traffic_simulator::lane_change::Constraint::Type::TIME: traj_with_goal = hdmap_utils->getLaneChangeTrajectory(lanelet_pose, lane_change_parameters_.value()); - along_pose = hdmap_utils->getAlongLaneletPose( + along_pose = traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( lanelet_pose, lane_change_parameters_->constraint.value); break; } @@ -150,10 +152,10 @@ BT::NodeStatus LaneChangeAction::tick() target_s_ = traj_with_goal->second; goal_pose.lanelet_id = lane_change_parameters_->target.lanelet_id; goal_pose.s = traj_with_goal->second; - double offset = std::fabs( - math::geometry::getRelativePose( - hdmap_utils->toMapPose(along_pose).pose, hdmap_utils->toMapPose(goal_pose).pose) - .position.y); + double offset = std::fabs(math::geometry::getRelativePose( + traffic_simulator::pose::toMapPose(along_pose), + traffic_simulator::pose::toMapPose(goal_pose)) + .position.y); switch (lane_change_parameters_->constraint.type) { case traffic_simulator::lane_change::Constraint::Type::NONE: lane_change_velocity_ = canonicalized_entity_status->getTwist().linear.x; @@ -254,7 +256,7 @@ BT::NodeStatus LaneChangeAction::tick() lanelet_pose.offset = 0; entity_status_updated.lanelet_pose = lanelet_pose; entity_status_updated.lanelet_pose_valid = true; - entity_status_updated.pose = hdmap_utils->toMapPose(lanelet_pose).pose; + entity_status_updated.pose = traffic_simulator::pose::toMapPose(lanelet_pose); entity_status_updated.action_status = canonicalized_entity_status->getActionStatus(); setCanonicalizedEntityStatus(entity_status_updated); return BT::NodeStatus::SUCCESS; diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index a0ea791a672..6c6076a3915 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,24 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* feat(traffic_simulator, openscenario_interpreter, behavior_tree_plugin, simple_sensor_simulator): use pose:: from lanelet_wrapper instead of hdmap_utils methods +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index 4e16f77445a..bb88081adfc 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 7.4.7 + 8.0.0 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/do_nothing_plugin/src/plugin.cpp b/simulation/do_nothing_plugin/src/plugin.cpp index af44aebae02..9a24230969b 100644 --- a/simulation/do_nothing_plugin/src/plugin.cpp +++ b/simulation/do_nothing_plugin/src/plugin.cpp @@ -181,7 +181,7 @@ void DoNothingBehavior::update(double current_time, double step_time) if (getRequest() == traffic_simulator::behavior::Request::FOLLOW_POLYLINE_TRAJECTORY) { canonicalized_entity_status_->set( interpolate_entity_status_on_polyline_trajectory(), - getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); + getDefaultMatchingDistanceForLaneletPoseCalculation()); if ( getCurrentTime() + getStepTime() >= do_nothing_behavior::follow_trajectory::getLastVertexTimestamp(getPolylineTrajectory())) { @@ -190,7 +190,7 @@ void DoNothingBehavior::update(double current_time, double step_time) } else { canonicalized_entity_status_->set( static_cast(*canonicalized_entity_status_), - getDefaultMatchingDistanceForLaneletPoseCalculation(), getHdMapUtils()); + getDefaultMatchingDistanceForLaneletPoseCalculation()); } } diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index f43a4837a7a..205002b8492 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,25 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* fix(traffic_simulator, simple_sensor_simulator): fix after merge, apply sonar recommendations +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* feat(traffic_simulator, openscenario_interpreter, behavior_tree_plugin, simple_sensor_simulator): use pose:: from lanelet_wrapper instead of hdmap_utils methods +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 5d1fa752b74..e2f6c6796dd 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 7.4.7 + 8.0.0 simple_sensor_simulator package masaya kataoka diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index ea98c5ad36b..8ec958aa763 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -83,6 +84,7 @@ auto ScenarioSimulator::initialize(const simulation_api_schema::InitializeReques step_time_ = req.step_time(); current_simulation_time_ = req.initialize_time(); current_scenario_time_ = std::numeric_limits::quiet_NaN(); + traffic_simulator::lanelet_map::activate(req.lanelet2_map_path()); builtin_interfaces::msg::Time t; simulation_interface::toMsg(req.initialize_ros_time(), t); current_ros_time_ = t; diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index b8c0fe8362f..2cbf0b0acb6 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace vehicle_simulation @@ -366,9 +367,12 @@ auto EgoEntitySimulation::calculateAccelerationBySlope() const -> double { if (consider_acceleration_by_road_slope_) { constexpr double gravity_acceleration = -9.81; + /// @todo why there is a need to recalculate orientation using getLaneletPose? + /// status_.getMapPose().orientation already contains the orientation const double ego_pitch_angle = math::geometry::convertQuaternionToEulerAngle( - hdmap_utils_ptr_->toMapPose(status_.getLaneletPose(), true).pose.orientation) + traffic_simulator::lanelet_wrapper::pose::toMapPose(status_.getLaneletPose(), true) + .pose.orientation) .y; return -std::sin(ego_pitch_angle) * gravity_acceleration; } else { @@ -453,8 +457,7 @@ auto EgoEntitySimulation::setStatus(const traffic_simulator_msgs::msg::EntitySta /// value from EntityStatus, therefore canonicalization has to be done in advance, /// not inside CanonicalizedEntityStatus const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - status.pose, status.bounding_box, unique_route_lanelets, false, matching_distance, - hdmap_utils_ptr_); + status.pose, status.bounding_box, unique_route_lanelets, false, matching_distance); status_.set(traffic_simulator::CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); setAutowareStatus(); } diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index 90c801d74da..42faf9bb479 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include using namespace vehicle_simulation; @@ -36,6 +38,10 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope) .longitude(-1.20294718763) .altitude(0.0)); + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/slope/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); + constexpr double gravity_acceleration = -9.81; // expected value in the lanelet(id:7) // first 25m: 1m up @@ -53,7 +59,8 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope) initial_status.name = "ego"; // use pitch-filled map pose initial_status.lanelet_pose_valid = false; - initial_status.pose = hdmap_utils->toMapPose(lanelet_pose, true).pose; + initial_status.pose = + traffic_simulator::lanelet_wrapper::pose::toMapPose(lanelet_pose, true).pose; EgoEntitySimulation ego_entity_simulation( initial_status, traffic_simulator_msgs::msg::VehicleParameters(), 1.f / 30.f, hdmap_utils, diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index 2a04b91a132..dad7a15e81f 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 081ed327775..51454ad1dc2 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 7.4.7 + 8.0.0 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 a9dc6b7d21d..9977046a1ef 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,104 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Add reader functions to cache classes to mitigate direct data members access and having to lock the mutex in different places +* Revert "Refactor lanelet_wrapper cache classes: make top level public member functions acquire resources and make non-public member functions NOT thread safe" + This reverts commit 41f9dd1e226ea827444aed1d01c207d10b9b61fa. +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Remove unnecessary comments +* Adjust comments to use "/// @note" convention +* Refactor lanelet_wrapper cache classes: make top level public member functions acquire resources and make non-public member functions NOT thread safe + This change is a proposal of improving thread safety - now the whole bodies of public member functions can be executed at once without the risk of resources being modified in the middle of the function execution +* Clean cache code - move cache classes member variables to the end of class declaration +* Move lanelet_wrapper cache classes member values and some functions to private +* Add comment explaining parameters +* Rename variable to match function name change +* Rename `borderlinePoses` -> `noNextLaneletPoses` +* Add comment explaining what `lanelet_map::borderlinePoses` does +* Perform calculations at compile time in lanelet_wrapper::pose::toLaneletPose +* Fix lanelet pose matching calculation bug +* fix(lanelet_wrapper): fix toLaneletPose +* doc(lanelet_wrapper): improve lanelet_wrappe::pose::canonicalizeLaneletPose description +* fix(lanelet_wrapper): fix pose toLaneletPose typo ";" +* fix(lanelet_wrapper): fix typo centerline +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(lanelet_wrapper): add toLaneletPose yaw_threshold description +* ref(traffic_simulator): add comments to lanelet_wrapper::pose, reduce the number of intermediate variables +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Update simulation/traffic_simulator/src/traffic/traffic_controller.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/utils/pose.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/utils/pose.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/src/utils/pose.cpp + Co-authored-by: Masaya Kataoka +* Update simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp + Co-authored-by: Masaya Kataoka +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): apply solar changes +* ref(traffic_simulator): apply sonar required changes +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): refactor laneletAltitude and isAltitudeMatching after merge +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* fix(traffic_simulator): fix follow_trajectory_action issue, add orientation to distance calc, remove toCanonicalizedLaneletPose(point...) because it can cause a another issues +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): improve hdmaputils::countLaneChanges +* fix(traffic_simulator): adapt lanelet-wapper::pose to changes in hdmap_utils +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* fix(traffic_simulator): fix lanelet_wrapper::pose::alternativeLaneletPoses +* fix(traffic_simulator, simple_sensor_simulator): fix after merge, apply sonar recommendations +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): apply sonar recommendations to lanelet_wrapper and utils +* feat(traffic_simulator): separate lanelet_loader, provide const TrafficRulesWithRoutingGraph +* ref(traffic_simulator): improve Configuration, traffic_rules, lanelet_wrapper +* fix(traffic_simulator): fix hdmap_utils tests +* fix(traffic_simulator): add EOF to traffic_rules +* fix(traffic_simmulator): fix spell check +* feat(traffic_simulator): use lanelet_wrapper::lanelet_map in the rest of hdmap_utils, adapt hdmap_utils test to lanelet_wrapper::lanelet_map +* feat(traffic_simulator, behavior_tree_plugin): use lanelet_wrapper::lanelet_map in parts previusly overlooked +* feat(traffic_simulator): use lanelet_wrapper::pose in the rest of hdmap_utils, adapt hdmap_utils test to lanelet_wrapper::pose +* feat(traffic_simulator, behavior_tree_plugin): use lanelet_wrapper::pose in parts previously overlooked +* ref(traffic_simulator): remove unused parts lanelet_wrapper +* feat(traffic_simulator, random_test_runner): adapt tests for using pose:: from lanelet_wrapper +* feat(traffic_simulator, openscenario_interpreter, behavior_tree_plugin, simple_sensor_simulator): use pose:: from lanelet_wrapper instead of hdmap_utils methods +* feat(traffic_simulator): add lanelet_wrapper as a replacement for hdmap_utils, at this point mainly for pose calculations +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index 9394ec74af8..f55539afe1f 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -45,21 +45,26 @@ ament_auto_add_library(traffic_simulator SHARED src/entity/pedestrian_entity.cpp src/entity/vehicle_entity.cpp src/hdmap_utils/hdmap_utils.cpp - src/hdmap_utils/traffic_rules.cpp src/helper/helper.cpp - src/job/job_list.cpp src/job/job.cpp + src/job/job_list.cpp + src/lanelet_wrapper/lanelet_loader.cpp + src/lanelet_wrapper/lanelet_map.cpp + src/lanelet_wrapper/lanelet_wrapper.cpp + src/lanelet_wrapper/pose.cpp + src/lanelet_wrapper/traffic_rules.cpp src/simulation_clock/simulation_clock.cpp + src/traffic/traffic_controller.cpp + src/traffic/traffic_sink.cpp + src/traffic/traffic_source.cpp src/traffic_lights/configurable_rate_updater.cpp + src/traffic_lights/traffic_light.cpp src/traffic_lights/traffic_light_marker_publisher.cpp src/traffic_lights/traffic_light_publisher.cpp - src/traffic_lights/traffic_light.cpp - src/traffic_lights/traffic_lights_base.cpp src/traffic_lights/traffic_lights.cpp - src/traffic/traffic_controller.cpp - src/traffic/traffic_sink.cpp - src/traffic/traffic_source.cpp + src/traffic_lights/traffic_lights_base.cpp src/utils/distance.cpp + src/utils/lanelet_map.cpp src/utils/pose.cpp ) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp index 22007c7c6bc..4be810be98d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/configuration.hpp @@ -23,6 +23,7 @@ #include #include #include +#include namespace traffic_simulator { @@ -32,17 +33,17 @@ struct Configuration using Pathname = boost::filesystem::path; - std::set auto_sink_entity_types = {}; - bool verbose = false; - bool standalone_mode = false; - std::string simulator_host = "localhost"; - double conventional_traffic_light_publish_rate = 30.0; + const bool standalone_mode = false; + + const double conventional_traffic_light_publish_rate = 30.0; - double v2i_traffic_light_publish_rate = 10.0; + const double v2i_traffic_light_publish_rate = 10.0; + + const std::set auto_sink_entity_types; /* ---- NOTE ----------------------------------------------------------------- * @@ -59,17 +60,34 @@ struct Configuration * ------------------------------------------------------------------------ */ const Pathname map_path; - Filename lanelet2_map_file; + const Filename lanelet2_map_file; - Filename pointcloud_map_file; + const Filename pointcloud_map_file; - Pathname scenario_path = ""; + const Pathname scenario_path; - explicit Configuration(const Pathname & map_path) // - : map_path(assertMapPath(map_path)), + explicit Configuration( + const Pathname & map_path, const Pathname & scenario_path, + const std::set & auto_sink_entity_types = {}) + : auto_sink_entity_types(auto_sink_entity_types), + map_path(assertMapPath(map_path)), lanelet2_map_file(findLexicographicallyFirstFilenameOf(map_path, ".osm")), - pointcloud_map_file(findLexicographicallyFirstFilenameOf(map_path, ".pcd")) + pointcloud_map_file(findLexicographicallyFirstFilenameOf(map_path, ".pcd")), + scenario_path(scenario_path) { + traffic_simulator::lanelet_map::activate(lanelet2_map_path().string()); + } + + explicit Configuration( + const Pathname & map_path, const Filename & lanelet2_map_file, const Pathname & scenario_path, + const std::set & auto_sink_entity_types = {}) + : auto_sink_entity_types(auto_sink_entity_types), + map_path(assertMapPath(map_path)), + lanelet2_map_file(lanelet2_map_file), + pointcloud_map_file(findLexicographicallyFirstFilenameOf(map_path, ".pcd")), + scenario_path(scenario_path) + { + traffic_simulator::lanelet_map::activate(lanelet2_map_path().string()); } auto assertMapPath(const Pathname & map_path) const -> const Pathname & @@ -131,9 +149,12 @@ struct Configuration } } - auto lanelet2_map_path() const { return map_path / lanelet2_map_file; } + auto lanelet2_map_path() const -> boost::filesystem::path { return map_path / lanelet2_map_file; } - auto pointcloud_map_path() const { return map_path / pointcloud_map_file; } + auto pointcloud_map_path() const -> boost::filesystem::path + { + return map_path / pointcloud_map_file; + } }; } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index b1d07af341f..38be01bbe99 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -39,11 +39,9 @@ class CanonicalizedEntityStatus auto set(const CanonicalizedEntityStatus & status) -> void; auto set( - const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void; - auto set( - const EntityStatus & status, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void; + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance) + -> void; + auto set(const EntityStatus & status, const double matching_distance) -> void; auto setAction(const std::string & action) -> void; auto getActionStatus() const noexcept -> const traffic_simulator_msgs::msg::ActionStatus &; diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp index 34d75b4e1f3..90e2fb11a2e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp @@ -15,7 +15,15 @@ #ifndef TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_ #define TRAFFIC_SIMULATOR__DATA_TYPE__LANELET_POSE_HPP_ +#include + +#include +#include +#include +#include +#include #include +#include namespace traffic_simulator { @@ -26,23 +34,25 @@ inline namespace lanelet_pose class CanonicalizedLaneletPose { public: + explicit CanonicalizedLaneletPose(const LaneletPose & non_canonicalized_lanelet_pose); explicit CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils); - explicit CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils); + const LaneletPose & non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets); CanonicalizedLaneletPose(const CanonicalizedLaneletPose & other); CanonicalizedLaneletPose(CanonicalizedLaneletPose && other) noexcept; CanonicalizedLaneletPose & operator=(const CanonicalizedLaneletPose & obj); explicit operator LaneletPose() const noexcept { return lanelet_pose_; } explicit operator geometry_msgs::msg::Pose() const noexcept { return map_pose_; } + auto getLaneletPose() const -> const LaneletPose & { return lanelet_pose_; } + auto getAltitude() const -> double { return map_pose_.position.z; } + auto getLaneletId() const noexcept -> lanelet::Id { return lanelet_pose_.lanelet_id; } + auto alignOrientationToLanelet() -> void; auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - const traffic_simulator::RoutingConfiguration & routing_configuration = - traffic_simulator::RoutingConfiguration()) const -> std::optional; + const RoutingConfiguration & routing_configuration = RoutingConfiguration()) const + -> std::optional; + static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void { consider_pose_by_road_slope_ = consider_pose_by_road_slope; @@ -72,14 +82,7 @@ class CanonicalizedLaneletPose #undef DEFINE_COMPARISON_OPERATOR private: - auto adjustOrientationAndOzPosition(const std::shared_ptr & hdmap_utils) - -> void; - auto canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils) -> LaneletPose; - auto canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils) -> LaneletPose; + auto adjustOrientationAndOzPosition() -> void; LaneletPose lanelet_pose_; std::vector lanelet_poses_; geometry_msgs::msg::Pose map_pose_; diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 75af7e4972a..21d67a9b5d0 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -372,7 +372,7 @@ class EntityManager } else if constexpr (std::is_same_v, geometry_msgs::msg::Pose>) { entity_status.pose = pose; const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - pose, parameters.bounding_box, include_crosswalk, matching_distance, hdmap_utils_ptr_); + pose, parameters.bounding_box, include_crosswalk, matching_distance); return CanonicalizedEntityStatus(entity_status, canonicalized_lanelet_pose); } }; diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp index 01efef8059d..fa20308d0cc 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/cache.hpp @@ -20,28 +20,10 @@ #include #include #include +#include #include #include -namespace std -{ -template <> -struct hash> -{ -public: - size_t operator()(const std::tuple & data) const - { - std::hash lanelet_id_hash; - size_t seed = 0; - // hash combine like boost library - seed ^= lanelet_id_hash(std::get<0>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - seed ^= lanelet_id_hash(std::get<1>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - seed ^= std::hash{}(std::get<2>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - return seed; - } -}; -} // namespace std - namespace hdmap_utils { class RouteCache @@ -49,7 +31,7 @@ class RouteCache public: auto exists(lanelet::Id from, lanelet::Id to, bool allow_lane_change) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); std::tuple key = {from, to, allow_lane_change}; return data_.find(key) != data_.end(); } @@ -62,7 +44,7 @@ class RouteCache "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), " lane change does not exists on route cache."); } else { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at({from, to, allow_lane_change}); } } @@ -71,7 +53,7 @@ class RouteCache lanelet::Id from, lanelet::Id to, const bool allow_lane_change, const lanelet::Ids & route) -> void { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[{from, to, allow_lane_change}] = route; } @@ -86,7 +68,7 @@ class CenterPointsCache public: auto exists(lanelet::Id lanelet_id) -> bool { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } @@ -95,7 +77,7 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.at(lanelet_id); } @@ -104,13 +86,13 @@ class CenterPointsCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return splines_[lanelet_id]; } auto appendData(lanelet::Id lanelet_id, const std::vector & route) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = route; splines_[lanelet_id] = std::make_shared(route); } @@ -128,7 +110,7 @@ class LaneletLengthCache public: auto exists(lanelet::Id lanelet_id) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_.find(lanelet_id) != data_.end(); } @@ -137,13 +119,13 @@ class LaneletLengthCache if (!exists(lanelet_id)) { THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); return data_[lanelet_id]; } auto appendData(lanelet::Id lanelet_id, double length) { - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); data_[lanelet_id] = length; } diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 02e6a893ad2..199b01890da 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -48,7 +48,6 @@ #include #include #include -#include #include #include #include @@ -71,15 +70,6 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingConfiguration().routing_graph_type) const -> bool; - auto canonicalizeLaneletPose(const traffic_simulator_msgs::msg::LaneletPose &) const - -> std::tuple< - std::optional, std::optional>; - - auto canonicalizeLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose &, const lanelet::Ids & route_lanelets) const - -> std::tuple< - std::optional, std::optional>; - auto clipTrajectoryFromLaneletIds( const lanelet::Id, const double s, const lanelet::Ids &, const double forward_distance = 20) const -> std::vector; @@ -94,15 +84,6 @@ class HdMapUtils auto generateMarker() const -> visualization_msgs::msg::MarkerArray; - auto getAllCanonicalizedLaneletPoses(const traffic_simulator_msgs::msg::LaneletPose &) const - -> std::vector; - - auto getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from, const double along, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> traffic_simulator_msgs::msg::LaneletPose; - auto getCenterPoints(const lanelet::Ids &) const -> std::vector; auto getCenterPoints(const lanelet::Id) const -> std::vector; @@ -188,8 +169,6 @@ class HdMapUtils auto getLaneletIds() const -> lanelet::Ids; - auto getLaneletLength(const lanelet::Id) const -> double; - auto getLaneletPolygon(const lanelet::Id) const -> std::vector; auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets; @@ -202,10 +181,6 @@ class HdMapUtils auto getLeftBound(const lanelet::Id) const -> std::vector; - auto getLeftLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType, - const bool include_opposite_direction) const -> lanelet::Ids; - auto getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, @@ -220,46 +195,6 @@ class HdMapUtils const geometry_msgs::msg::Point &, const double distance_threshold, const std::size_t search_count = 5) const -> lanelet::Ids; - auto getNextLaneletIds( - const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getNextLaneletIds( - const lanelet::Ids &, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - - auto getNextLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getNextLaneletIds( - const lanelet::Id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Ids &, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> lanelet::Ids; - - auto getPreviousLaneletIds( - const lanelet::Id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids; - auto getPreviousLanelets( const lanelet::Id, const double backward_horizon = 100, const traffic_simulator::RoutingGraphType type = @@ -267,10 +202,6 @@ class HdMapUtils auto getRightBound(const lanelet::Id) const -> std::vector; - auto getRightLaneletIds( - const lanelet::Id, const traffic_simulator::RoutingGraphType, - const bool include_opposite_direction) const -> lanelet::Ids; - auto getRightOfWayLaneletIds(const lanelet::Ids &) const -> std::unordered_map; @@ -331,64 +262,11 @@ class HdMapUtils constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; public: - auto matchToLane( - const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0, - const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const bool include_crosswalk, - const double matching_distance = 1.0) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const lanelet::Ids &, - const double matching_distance = 1.0) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Point &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::optional; - - auto toLaneletPose( - const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 1.0) const - -> std::optional; - - auto toLaneletPoses( - const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 5.0, - const bool include_opposite_direction = false, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingConfiguration().routing_graph_type) const - -> std::vector; - auto toMapBin() const -> autoware_map_msgs::msg::LaneletMapBin; auto toMapPoints(const lanelet::Id, const std::vector & s) const -> std::vector; - auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = true) - const -> geometry_msgs::msg::PoseStamped; - - auto isAltitudeMatching(const double current_altitude, const double target_altitude) const - -> bool; - - auto getLaneletAltitude( - const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, - const double matching_distance = 1.0) const -> std::optional; - private: /** @defgroup cache * Declared mutable for caching diff --git a/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp b/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp index e10656fc00a..c86e3723c3c 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/helper/helper.hpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include @@ -67,12 +66,10 @@ LaneletPose constructLaneletPose( * @param lanelet_id lanelet id * @param s s value in lane coordinate * @param offset offset value in lane coordinate - * @param hdmap_utils_ptr pointer to HdmapUtils * @return LaneletPose */ -auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose; +auto constructCanonicalizedLaneletPose(lanelet::Id lanelet_id, double s, double offset) + -> CanonicalizedLaneletPose; /** * @brief helper function for constructing canonicalized lanelet pose @@ -83,12 +80,11 @@ auto constructCanonicalizedLaneletPose( * @param roll roll value in the lane coordinate * @param pitch pitch value in the lane coordinate * @param yaw yaw value in the lane coordinate - * @param hdmap_utils_ptr pointer to HdmapUtils * @return LaneletPose */ auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose; + lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw) + -> CanonicalizedLaneletPose; /** * @brief helper function for constructing rpy @@ -157,7 +153,8 @@ const simulation_api_schema::DetectionSensorConfiguration constructDetectionSens } // namespace helper } // namespace traffic_simulator -std::ostream & operator<<(std::ostream & os, const traffic_simulator::LaneletPose & ll_pose); +std::ostream & operator<<( + std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose); std::ostream & operator<<(std::ostream & os, const geometry_msgs::msg::Point & point); diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_loader.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_loader.hpp new file mode 100644 index 00000000000..d3da002a735 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_loader.hpp @@ -0,0 +1,40 @@ +// 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__LANELET_LOADER_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_LOADER_HPP_ + +#include + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +class LaneletLoader +{ +public: + static auto load(const std::filesystem::path & lanelet_map_path) -> lanelet::LaneletMapPtr; + +private: + static auto overwriteLaneletsCenterline(lanelet::LaneletMapPtr) -> void; + static auto resamplePoints( + const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) + -> lanelet::BasicPoints3d; + static auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector; +}; +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_LOADER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp new file mode 100644 index 00000000000..ebda8439df7 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp @@ -0,0 +1,95 @@ +// 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__LANELET_WRAPPER_LANELET_MAP_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANELET_MAP_HPP_ + +#include +#include + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace lanelet_map +{ +auto isInLanelet(const lanelet::Id lanelet_id, const double lanelet_pose_s) -> bool; + +auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool; + +auto laneletLength(const lanelet::Id lanelet_id) -> double; + +auto laneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance) -> std::optional; + +template +auto laneletIds(const std::vector & lanelets) -> lanelet::Ids +{ + lanelet::Ids ids; + std::transform( + lanelets.begin(), lanelets.end(), std::back_inserter(ids), + [](const auto & lanelet) { return lanelet.id(); }); + return ids; +} + +auto laneletIds() -> lanelet::Ids; + +auto nearbyLaneletIds( + const Point & point, const double distance_threshold, const bool include_crosswalk, + const std::size_t search_count) -> lanelet::Ids; + +auto centerPoints(const lanelet::Ids & lanelet_ids) -> std::vector; + +auto centerPoints(const lanelet::Id lanelet_id) -> std::vector; + +auto centerPointsSpline(const lanelet::Id lanelet_id) -> std::shared_ptr; + +auto nextLaneletIds( + const lanelet::Id lanelet_id, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto nextLaneletIds( + const lanelet::Ids & lanelet_ids, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto nextLaneletIds( + const lanelet::Id lanelet_id, std::string_view turn_direction, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto nextLaneletIds( + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto previousLaneletIds( + const lanelet::Id lanelet_id, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto previousLaneletIds( + const lanelet::Ids & lanelet_ids, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto previousLaneletIds( + const lanelet::Id lanelet_id, std::string_view turn_direction, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; + +auto previousLaneletIds( + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids; +} // namespace lanelet_map +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_LANELET_MAP_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp new file mode 100644 index 00000000000..b0bfca88a94 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp @@ -0,0 +1,323 @@ +// 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__LANELET_WRAPPER_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash> +{ +public: + size_t operator()(const std::tuple & data) const + { + std::hash lanelet_id_hash; + size_t seed = 0; + /// @note hash combine like boost library 2^32 / phi = 0x9e3779b9 + seed ^= lanelet_id_hash(std::get<0>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + seed ^= lanelet_id_hash(std::get<1>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + seed ^= std::hash{}(std::get<2>(data)) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + return seed; + } +}; +} // namespace std + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +using BoundingBox = traffic_simulator_msgs::msg::BoundingBox; +using EntityType = traffic_simulator_msgs::msg::EntityType; +using LaneletPose = traffic_simulator_msgs::msg::LaneletPose; +using Point = geometry_msgs::msg::Point; +using Pose = geometry_msgs::msg::Pose; +using PoseStamped = geometry_msgs::msg::PoseStamped; +using Spline = math::geometry::CatmullRomSpline; +using Vector3 = geometry_msgs::msg::Vector3; + +class RouteCache +{ +public: + auto getRoute( + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const lanelet::LaneletMapPtr & lanelet_map, const RoutingConfiguration & routing_configuration, + const lanelet::routing::RoutingGraphConstPtr & routing_graph) -> lanelet::Ids + { + if (!exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { + /// @note to use default routing costs: distance along lanelets + constexpr int routing_cost_id = 0; + const auto & from_lanelet = lanelet_map->laneletLayer.get(from_lanelet_id); + const auto & to_lanelet = lanelet_map->laneletLayer.get(to_lanelet_id); + if (const auto route = routing_graph->getRoute( + from_lanelet, to_lanelet, routing_cost_id, routing_configuration.allow_lane_change); + !route || route->shortestPath().empty()) { + appendData( + from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, lanelet::Ids()); + } else { + lanelet::Ids shortest_path_ids; + for (const auto & lanelet : route->shortestPath()) { + shortest_path_ids.push_back(lanelet.id()); + } + appendData( + from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, + shortest_path_ids); + } + } + return readData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change); + } + + auto getRoute(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) + -> decltype(auto) + { + if (!exists(from, to, allow_lane_change)) { + THROW_SIMULATION_ERROR( + "route from : ", from, " to : ", to, (allow_lane_change ? " with" : " without"), + " lane change does not exists on route cache."); + } else { + return readData(from, to, allow_lane_change); + } + } + +private: + auto exists(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) -> bool + { + std::lock_guard lock(mutex_); + std::tuple key = {from, to, allow_lane_change}; + return data_.find(key) != data_.end(); + } + + auto readData(const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change) + -> lanelet::Ids + { + std::lock_guard lock(mutex_); + return data_.at({from, to, allow_lane_change}); + } + + auto appendData( + const lanelet::Id from, const lanelet::Id to, const bool allow_lane_change, + const lanelet::Ids & route) -> void + { + std::lock_guard lock(mutex_); + data_[{from, to, allow_lane_change}] = route; + } + + std::unordered_map, lanelet::Ids> data_; + std::mutex mutex_; +}; + +class CenterPointsCache +{ +public: + auto centerPoints(lanelet::Id lanelet_id) -> decltype(auto) + { + if (!exists(lanelet_id)) { + THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); + } + return readData(lanelet_id); + } + + auto centerPointsSpline(lanelet::Id lanelet_id) -> decltype(auto) + { + if (!exists(lanelet_id)) { + THROW_SIMULATION_ERROR("center point of : ", lanelet_id, " does not exists on route cache."); + } + return readDataSpline(lanelet_id); + } + + auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) + -> std::vector + { + if (!exists(lanelet_id)) { + appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); + } + return readData(lanelet_id); + } + + auto getCenterPointsSpline( + const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) + -> std::shared_ptr + { + if (!exists(lanelet_id)) { + appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map)); + } + return readDataSpline(lanelet_id); + } + +private: + auto exists(const lanelet::Id lanelet_id) -> bool + { + std::lock_guard lock(mutex_); + return data_.find(lanelet_id) != data_.end(); + } + + auto readData(const lanelet::Id lanelet_id) -> std::vector + { + std::lock_guard lock(mutex_); + return data_.at(lanelet_id); + } + + auto readDataSpline(const lanelet::Id lanelet_id) -> std::shared_ptr + { + std::lock_guard lock(mutex_); + return splines_.at(lanelet_id); + } + + auto appendData(const lanelet::Id lanelet_id, const std::vector & route) -> void + { + std::lock_guard lock(mutex_); + data_[lanelet_id] = route; + splines_[lanelet_id] = std::make_shared(route); + } + + auto centerPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) const + -> std::vector + { + std::vector center_points; + for (const auto & point : lanelet_map->laneletLayer.get(lanelet_id).centerline()) { + center_points.push_back(geometry_msgs::build().x(point.x()).y(point.y()).z(point.z())); + } + if (center_points.size() == 2) { + const auto p0 = center_points[0]; + const auto p2 = center_points[1]; + const auto p1 = geometry_msgs::build() + .x((p0.x + p2.x) * 0.5) + .y((p0.y + p2.y) * 0.5) + .z((p0.z + p2.z) * 0.5); + center_points.clear(); + center_points.push_back(p0); + center_points.push_back(p1); + center_points.push_back(p2); + } + return center_points; + } + + std::unordered_map> data_; + std::unordered_map> splines_; + std::mutex mutex_; +}; + +class LaneletLengthCache +{ +public: + auto getLength(lanelet::Id lanelet_id) + { + if (!exists(lanelet_id)) { + THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache."); + } + return readData(lanelet_id); + } + + auto getLength(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> double + { + if (!exists(lanelet_id)) { + appendData( + lanelet_id, lanelet::utils::getLaneletLength2d(lanelet_map->laneletLayer.get(lanelet_id))); + } + return readData(lanelet_id); + } + +private: + auto exists(const lanelet::Id lanelet_id) -> bool + { + std::lock_guard lock(mutex_); + return data_.find(lanelet_id) != data_.end(); + } + + auto readData(const lanelet::Id lanelet_id) -> double + { + std::lock_guard lock(mutex_); + return data_.at(lanelet_id); + } + + auto appendData(const lanelet::Id lanelet_id, double length) -> void + { + std::lock_guard lock(mutex_); + data_[lanelet_id] = length; + } + + std::unordered_map data_; + std::mutex mutex_; +}; + +struct TrafficRulesWithRoutingGraph +{ + lanelet::traffic_rules::TrafficRulesPtr rules; + lanelet::routing::RoutingGraphConstPtr graph; + mutable RouteCache route_cache; + + TrafficRulesWithRoutingGraph( + const lanelet::LaneletMapPtr lanelet_map_ptr, const std::string & locations, + const std::string & participants) + { + rules = lanelet::traffic_rules::TrafficRulesFactory::create(locations, participants); + graph = lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *rules); + } +}; + +class LaneletWrapper +{ +public: + static auto activate(const std::string & lanelet_map_path) -> void; + + [[nodiscard]] static auto map() -> lanelet::LaneletMapPtr; + [[nodiscard]] static auto routingGraph(const RoutingGraphType type) + -> lanelet::routing::RoutingGraphConstPtr; + [[nodiscard]] static auto trafficRules(const RoutingGraphType type) + -> lanelet::traffic_rules::TrafficRulesPtr; + + [[nodiscard]] static auto routeCache(const RoutingGraphType type) -> RouteCache &; + [[nodiscard]] static auto centerPointsCache() -> CenterPointsCache &; + [[nodiscard]] static auto laneletLengthCache() -> LaneletLengthCache &; + +private: + explicit LaneletWrapper(const std::filesystem::path & lanelet_map_path); + static LaneletWrapper & getInstance(); + + inline static std::unique_ptr instance{nullptr}; + inline static std::string lanelet_map_path_{""}; + inline static std::mutex mutex_; + + const lanelet::LaneletMapPtr lanelet_map_ptr_; + + const TrafficRulesWithRoutingGraph vehicle_; + const TrafficRulesWithRoutingGraph vehicle_with_road_shoulder_; + const TrafficRulesWithRoutingGraph pedestrian_; + + mutable CenterPointsCache center_points_cache_; + mutable LaneletLengthCache lanelet_length_cache_; +}; +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp new file mode 100644 index 00000000000..278dbee3c47 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp @@ -0,0 +1,112 @@ +// 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__LANELET_WRAPPER_POSE_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_POSE_HPP_ + +#include + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace pose +{ +/// @note This value was determined experimentally by @hakuturu583 and not theoretically. +/// @sa https://github.com/tier4/scenario_simulator_v2/commit/4c8e9f496b061b00bec799159d59c33f2ba46b3a +constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8; + +auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch = true) -> PoseStamped; + +auto isAltitudeMatching(const double current_altitude, const double target_altitude) -> bool; + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance = 1.0) + -> std::optional; + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Ids & lanelet_ids, const double matching_distance = 1.0) + -> std::optional; + +auto toLaneletPose( + const Pose & map_pose, const bool include_crosswalk, const double matching_distance = 1.0) + -> std::optional; + +auto toLaneletPose( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance = 1.0, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) + -> std::optional; + +auto toLaneletPoses( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance = 5.0, + const bool include_opposite_direction = true, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) + -> std::vector; + +/** + * @brief Retrieves alternative lanelet poses based on the reference lanelet pose. + * + * This method computes alternative lanelet poses in the previous and next lanelets + * relative to a given reference lanelet pose. It recursively explores the neighboring + * lanelets until no further alternatives are found. The decision of whether a pose belongs + * to a previous or next lanelet is based on the `s` value of the reference pose: + * - If `s` is negative, the pose is assumed to be on the previous lanelet. + * - If `s` exceeds the lanelet length, the pose is assumed to be on the next lanelet. + * - If `s` is within the valid range of the lanelet (from 0 to the lanelet's length), + * the reference lanelet pose is returned directly. + * + * @param reference_lanelet_pose The reference pose on a lanelet, used to determine its position + * and compute alternatives in neighboring lanelets. + * + * @return A vector of alternative `LaneletPose` objects representing poses in the neighboring + * lanelets, or the reference pose if no alternatives are found. + */ +auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) + -> std::vector; + +auto alongLaneletPose( + const LaneletPose & from_pose, const lanelet::Ids & route_lanelets, const double distance) + -> LaneletPose; + +auto alongLaneletPose( + const LaneletPose & from_pose, const double distance, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> LaneletPose; + +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose) + -> std::tuple, std::optional>; + +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> std::tuple, std::optional>; + +auto matchToLane( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance = 1.0, + const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, + const RoutingGraphType type = RoutingConfiguration().routing_graph_type) + -> std::optional; + +auto leftLaneletIds( + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids; + +auto rightLaneletIds( + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids; +} // namespace pose +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_POSE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp similarity index 86% rename from simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp rename to simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp index d16133ee5f0..0ea608c2999 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/traffic_rules.hpp @@ -17,7 +17,9 @@ #include -namespace hdmap_utils +namespace traffic_simulator +{ +namespace lanelet_wrapper { struct Locations { @@ -29,16 +31,17 @@ struct Locations class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanVehicle { public: + using lanelet::traffic_rules::GenericTrafficRules::canPass; using lanelet::traffic_rules::GermanVehicle::GermanVehicle; protected: /// @note this function overrides and adds road shoulder handling to GenericTrafficRules::canPass - lanelet::Optional canPass( - const std::string & type, const std::string & /*location*/) const override + auto canPass(const std::string & type, const std::string & /*location*/) const + -> lanelet::Optional override { using lanelet::AttributeValueString; using lanelet::Participants; - const static std::map> participants_map{ + const static std::map, std::less<>> participants_map{ // clang-format off {"", {Participants::Vehicle}}, {AttributeValueString::Road, {Participants::Vehicle, Participants::Bicycle}}, @@ -59,7 +62,7 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV return {}; } - auto startsWith = [](const std::string & str, const std::string & substr) { + auto startsWith = [](std::string_view str, std::string_view substr) { return str.compare(0, substr.size(), substr) == 0; }; return lanelet::utils::anyOf(participants->second, [this, startsWith](auto & participant) { @@ -70,5 +73,6 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV lanelet::traffic_rules::LaneChangeType laneChangeType( const lanelet::ConstLineString3d &, bool) const override; }; -} // namespace hdmap_utils +} // namespace lanelet_wrapper +} // namespace traffic_simulator #endif // TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp new file mode 100644 index 00000000000..94e87836ad3 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/lanelet_map.hpp @@ -0,0 +1,51 @@ +// 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__UTILS__LANELET_MAP_HPP_ +#define TRAFFIC_SIMULATOR__UTILS__LANELET_MAP_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +inline namespace lanelet_map +{ +using Point = geometry_msgs::msg::Point; +using Pose = geometry_msgs::msg::Pose; + +template +inline auto activate(Ts &&... xs) +{ + return lanelet_wrapper::LaneletWrapper::activate(std::forward(xs)...); +} + +auto laneletLength(const lanelet::Id lanelet_id) -> double; + +template +inline auto laneletAltitude(Ts &&... xs) +{ + return lanelet_wrapper::lanelet_map::laneletAltitude(std::forward(xs)...); +} + +/// @brief Calculates all poses on the map that have no next lanelet (dead ends) +/// @return A vector of final poses and their corresponding lanelet IDs +auto noNextLaneletPoses() -> std::vector>; +} // namespace lanelet_map +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__UTILS__LANELET_MAP_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 369d81eaac2..764dce4f483 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -16,45 +16,45 @@ #define TRAFFIC_SIMULATOR__UTILS__POSE_HPP_ #include +#include namespace traffic_simulator { inline namespace pose { -// Useful constructors auto quietNaNPose() -> geometry_msgs::msg::Pose; auto quietNaNLaneletPose() -> LaneletPose; // Conversions -auto canonicalize( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; +auto canonicalize(const LaneletPose & lanelet_pose) -> LaneletPose; + +auto canonicalize(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> LaneletPose; auto toMapPose(const CanonicalizedLaneletPose & lanelet_pose) -> geometry_msgs::msg::Pose; -auto toMapPose( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) -> geometry_msgs::msg::Pose; +auto toMapPose(const LaneletPose & lanelet_pose) -> geometry_msgs::msg::Pose; + +auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector; + +auto toCanonicalizedLaneletPose(const LaneletPose & lanelet_pose) + -> std::optional; auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk, - const std::shared_ptr & hdmap_utils_ptr) + const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk) -> std::optional; auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const double matching_distance) -> std::optional; auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const double matching_distance) -> std::optional; auto transformRelativePoseToGlobal( const geometry_msgs::msg::Pose & global_pose, const geometry_msgs::msg::Pose & relative_pose) @@ -85,9 +85,13 @@ auto boundingBoxRelativePose( -> std::optional; // Relative LaneletPose +auto isAltitudeMatching( + const CanonicalizedLaneletPose & lanelet_pose, + const CanonicalizedLaneletPose & target_lanelet_pose) -> bool; + auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration, + const RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; auto boundingBoxRelativeLaneletPose( @@ -95,7 +99,7 @@ auto boundingBoxRelativeLaneletPose( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, - const traffic_simulator::RoutingConfiguration & routing_configuration, + const RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; // Others @@ -103,23 +107,19 @@ auto isInLanelet( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, const double tolerance, const std::shared_ptr & hdmap_utils_ptr) -> bool; +auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lanelet_id) -> bool; + auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool; -// it will be moved to "lanelet" namespace -auto laneletLength( - const lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) - -> double; - namespace pedestrian { auto transformToCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const double matching_distance) -> std::optional; } // namespace pedestrian } // namespace pose } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index 23ed8e72257..2bff9713b3f 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 7.4.7 + 8.0.0 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator/src/api/api.cpp b/simulation/traffic_simulator/src/api/api.cpp index 21ff6e498d7..b9f04a11024 100644 --- a/simulation/traffic_simulator/src/api/api.cpp +++ b/simulation/traffic_simulator/src/api/api.cpp @@ -152,9 +152,7 @@ auto API::setEntityStatus( const std::string & name, const LaneletPose & lanelet_pose, const traffic_simulator_msgs::msg::ActionStatus & action_status) -> void { - if ( - const auto canonicalized_lanelet_pose = - pose::canonicalize(lanelet_pose, entity_manager_ptr_->getHdmapUtils())) { + if (const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(lanelet_pose)) { setEntityStatus(name, canonicalized_lanelet_pose.value(), action_status); } else { std::stringstream ss; diff --git a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp index 5f6341afa6e..0f2c0af28c9 100644 --- a/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp +++ b/simulation/traffic_simulator/src/behavior/follow_trajectory.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include #include #include @@ -74,14 +74,24 @@ auto makeUpdatedStatus( auto distance_along_lanelet = [&](const geometry_msgs::msg::Point & from, const geometry_msgs::msg::Point & to) -> double { - if (const auto from_lanelet_pose = - hdmap_utils->toLaneletPose(from, entity_status.bounding_box, false, matching_distance); + const auto quaternion = math::geometry::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); + const auto to_pose = + geometry_msgs::build().position(to).orientation(quaternion); + if (const auto from_lanelet_pose = pose::toCanonicalizedLaneletPose( + from_pose, entity_status.bounding_box, false, matching_distance); from_lanelet_pose) { - if (const auto to_lanelet_pose = - hdmap_utils->toLaneletPose(to, entity_status.bounding_box, false, matching_distance); + if (const auto to_lanelet_pose = pose::toCanonicalizedLaneletPose( + to_pose, entity_status.bounding_box, false, matching_distance); to_lanelet_pose) { if (const auto distance = hdmap_utils->getLongitudinalDistance( - from_lanelet_pose.value(), to_lanelet_pose.value()); + static_cast(from_lanelet_pose.value()), + static_cast(to_lanelet_pose.value())); distance) { return distance.value(); } @@ -565,13 +575,8 @@ auto makeUpdatedStatus( // Do not change orientation if there is no designed_velocity vector return entity_status.pose.orientation; } else { - // If there is a designed_velocity vector, set the orientation in the direction of it - const geometry_msgs::msg::Vector3 direction = - geometry_msgs::build() - .x(0.0) - .y(std::atan2(-desired_velocity.z, std::hypot(desired_velocity.x, desired_velocity.y))) - .z(std::atan2(desired_velocity.y, desired_velocity.x)); - return math::geometry::convertEulerAngleToQuaternion(direction); + // if there is a designed_velocity vector, set the orientation in the direction of it + return math::geometry::convertDirectionToQuaternion(desired_velocity); } }(); diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 758d10b6abf..4d32f1a71f4 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -64,8 +64,8 @@ auto CanonicalizedEntityStatus::set(const CanonicalizedEntityStatus & status) -> } auto CanonicalizedEntityStatus::set( - const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void + const EntityStatus & status, const lanelet::Ids & lanelet_ids, const double matching_distance) + -> void { const auto include_crosswalk = getType().type == traffic_simulator_msgs::msg::EntityType::PEDESTRIAN || @@ -73,21 +73,19 @@ auto CanonicalizedEntityStatus::set( std::optional canonicalized_lanelet_pose; if (status.lanelet_pose_valid) { - canonicalized_lanelet_pose = pose::canonicalize(status.lanelet_pose, hdmap_utils_ptr); + canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(status.lanelet_pose); } else { // prefer the current lanelet canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( - status.pose, getBoundingBox(), lanelet_ids, include_crosswalk, matching_distance, - hdmap_utils_ptr); + status.pose, getBoundingBox(), lanelet_ids, include_crosswalk, matching_distance); } set(CanonicalizedEntityStatus(status, canonicalized_lanelet_pose)); } -auto CanonicalizedEntityStatus::set( - const EntityStatus & status, const double matching_distance, - const std::shared_ptr & hdmap_utils_ptr) -> void +auto CanonicalizedEntityStatus::set(const EntityStatus & status, const double matching_distance) + -> void { - set(status, getLaneletIds(), matching_distance, hdmap_utils_ptr); + set(status, getLaneletIds(), matching_distance); } auto CanonicalizedEntityStatus::setAction(const std::string & action) -> void @@ -124,7 +122,8 @@ auto CanonicalizedEntityStatus::getMapPose() const noexcept -> const geometry_ms auto CanonicalizedEntityStatus::getAltitude() const -> double { - return entity_status_.pose.position.z; + return canonicalized_lanelet_pose_ ? canonicalized_lanelet_pose_->getAltitude() + : entity_status_.pose.position.z; } auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index b4dfb4dc0d8..121bd44af5c 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include namespace traffic_simulator @@ -25,25 +26,21 @@ namespace traffic_simulator inline namespace lanelet_pose { CanonicalizedLaneletPose::CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils) -: lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, hdmap_utils)), - lanelet_poses_( - hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) + const LaneletPose & non_canonicalized_lanelet_pose) +: lanelet_pose_(pose::canonicalize(non_canonicalized_lanelet_pose)), + lanelet_poses_(pose::alternativeLaneletPoses(non_canonicalized_lanelet_pose)), + map_pose_(pose::toMapPose(lanelet_pose_)) { - adjustOrientationAndOzPosition(hdmap_utils); + adjustOrientationAndOzPosition(); } CanonicalizedLaneletPose::CanonicalizedLaneletPose( - const LaneletPose & maybe_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils) -: lanelet_pose_(canonicalize(maybe_non_canonicalized_lanelet_pose, route_lanelets, hdmap_utils)), - lanelet_poses_( - hdmap_utils->getAllCanonicalizedLaneletPoses(maybe_non_canonicalized_lanelet_pose)), - map_pose_(pose::toMapPose(lanelet_pose_, hdmap_utils)) + const LaneletPose & non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets) +: lanelet_pose_(pose::canonicalize(non_canonicalized_lanelet_pose, route_lanelets)), + lanelet_poses_(pose::alternativeLaneletPoses(non_canonicalized_lanelet_pose)), + map_pose_(pose::toMapPose(lanelet_pose_)) { - adjustOrientationAndOzPosition(hdmap_utils); + adjustOrientationAndOzPosition(); } CanonicalizedLaneletPose::CanonicalizedLaneletPose(const CanonicalizedLaneletPose & other) @@ -69,50 +66,9 @@ auto CanonicalizedLaneletPose::operator=(const CanonicalizedLaneletPose & other) return *this; } -auto CanonicalizedLaneletPose::canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, - const std::shared_ptr & hdmap_utils) -> LaneletPose -{ - if ( - const auto canonicalized = std::get>( - hdmap_utils->canonicalizeLaneletPose(may_non_canonicalized_lanelet_pose))) { - return canonicalized.value(); - } else { - THROW_SEMANTIC_ERROR( - "Lanelet pose (id=", may_non_canonicalized_lanelet_pose.lanelet_id, - ",s=", may_non_canonicalized_lanelet_pose.s, - ",offset=", may_non_canonicalized_lanelet_pose.offset, - ",rpy.x=", may_non_canonicalized_lanelet_pose.rpy.x, - ",rpy.y=", may_non_canonicalized_lanelet_pose.rpy.y, - ",rpy.z=", may_non_canonicalized_lanelet_pose.rpy.z, - ") is invalid, please check lanelet length and connection."); - } -} - -auto CanonicalizedLaneletPose::canonicalize( - const LaneletPose & may_non_canonicalized_lanelet_pose, const lanelet::Ids & route_lanelets, - const std::shared_ptr & hdmap_utils) -> LaneletPose -{ - if ( - const auto canonicalized = std::get>( - hdmap_utils->canonicalizeLaneletPose(may_non_canonicalized_lanelet_pose, route_lanelets))) { - return canonicalized.value(); - } else { - THROW_SEMANTIC_ERROR( - "Lanelet pose (id=", may_non_canonicalized_lanelet_pose.lanelet_id, - ",s=", may_non_canonicalized_lanelet_pose.s, - ",offset=", may_non_canonicalized_lanelet_pose.offset, - ",rpy.x=", may_non_canonicalized_lanelet_pose.rpy.x, - ",rpy.y=", may_non_canonicalized_lanelet_pose.rpy.y, - ",rpy.z=", may_non_canonicalized_lanelet_pose.rpy.z, - ") is invalid, please check lanelet length, connection and entity route."); - } -} - auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - const traffic_simulator::RoutingConfiguration & routing_configuration) const - -> std::optional + const RoutingConfiguration & routing_configuration) const -> std::optional { if (lanelet_poses_.empty()) { return std::nullopt; @@ -131,14 +87,32 @@ auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( return alternative_lanelet_pose; } -auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition( - const std::shared_ptr & hdmap_utils) -> void +auto CanonicalizedLaneletPose::alignOrientationToLanelet() -> void +{ + using math::geometry::convertEulerAngleToQuaternion; + using math::geometry::convertQuaternionToEulerAngle; + /// @todo it will be changed to route::toSpline(...) + const auto lanelet_rpy = convertQuaternionToEulerAngle( + math::geometry::CatmullRomSpline( + lanelet_wrapper::lanelet_map::centerPoints({lanelet_pose_.lanelet_id})) + .getPose(lanelet_pose_.s, true) + .orientation); + map_pose_.orientation = + convertEulerAngleToQuaternion(geometry_msgs::build() + .x(lanelet_rpy.x) + .y(lanelet_rpy.y) + .z(lanelet_rpy.z)); + lanelet_pose_.rpy = geometry_msgs::build().x(0.0).y(0.0).z(0.0); +} + +auto CanonicalizedLaneletPose::adjustOrientationAndOzPosition() -> void { using math::geometry::convertEulerAngleToQuaternion; using math::geometry::convertQuaternionToEulerAngle; using math::geometry::getRotation; - const math::geometry::CatmullRomSpline spline( - hdmap_utils->getCenterPoints(lanelet_pose_.lanelet_id)); + /// @todo it will be changed to route::toSpline(...) + const auto spline = math::geometry::CatmullRomSpline( + lanelet_wrapper::lanelet_map::centerPoints({lanelet_pose_.lanelet_id})); // adjust Oz position if (const auto s_value = spline.getSValue(map_pose_)) { map_pose_.position.z = spline.getPoint(s_value.value()).z; diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index abb85f7885f..12141f1fb3f 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -284,7 +284,7 @@ auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void // prefer current lanelet on Autoware side status_->set( entity_status, helper::getUniqueValues(getRouteLanelets()), - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + getDefaultMatchingDistanceForLaneletPoseCalculation()); } } // namespace entity } // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 139fbd51a2d..a0c7b5aecc1 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -99,7 +99,7 @@ auto EntityBase::getCanonicalizedLaneletPose(const double matching_distance) con // prefer the current lanelet return pose::toCanonicalizedLaneletPose( status_->getMapPose(), status_->getBoundingBox(), status_->getLaneletIds(), include_crosswalk, - matching_distance, hdmap_utils_ptr_); + matching_distance); } auto EntityBase::getDefaultMatchingDistanceForLaneletPoseCalculation() const -> double @@ -545,13 +545,12 @@ void EntityBase::setOtherStatus( auto EntityBase::setStatus(const EntityStatus & status, const lanelet::Ids & lanelet_ids) -> void { - status_->set( - status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + status_->set(status, lanelet_ids, getDefaultMatchingDistanceForLaneletPoseCalculation()); } auto EntityBase::setStatus(const EntityStatus & status) -> void { - status_->set(status, getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_); + status_->set(status, getDefaultMatchingDistanceForLaneletPoseCalculation()); } auto EntityBase::setCanonicalizedStatus(const CanonicalizedEntityStatus & status) -> void @@ -734,8 +733,8 @@ auto EntityBase::requestSynchronize( : other_status_.find(target_name)->second.getLaneletPose(); const auto target_entity_distance = longitudinalDistance( - CanonicalizedLaneletPose(target_entity_lanelet_pose, hdmap_utils_ptr_), target_sync_pose, - true, false, lane_changeable_routing_configuration, hdmap_utils_ptr_); + CanonicalizedLaneletPose(target_entity_lanelet_pose), target_sync_pose, true, false, + lane_changeable_routing_configuration, hdmap_utils_ptr_); if (!target_entity_distance.has_value() || target_entity_distance.value() < 0.0) { RCLCPP_WARN_ONCE( rclcpp::get_logger("traffic_simulator"), diff --git a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp index da7ec5198e4..292c918e86d 100644 --- a/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp +++ b/simulation/traffic_simulator/src/entity/pedestrian_entity.cpp @@ -72,7 +72,7 @@ void PedestrianEntity::requestAssignRoute(const std::vectorgetBoundingBox(), true, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Waypoint of pedestrian entity should be on lane."); @@ -148,7 +148,7 @@ void PedestrianEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & m if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( map_pose, status_->getBoundingBox(), true, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Goal of the pedestrian entity should be on lane."); diff --git a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp index e67fccac006..59df55b01c2 100644 --- a/simulation/traffic_simulator/src/entity/vehicle_entity.cpp +++ b/simulation/traffic_simulator/src/entity/vehicle_entity.cpp @@ -174,7 +174,7 @@ void VehicleEntity::requestAcquirePosition(const geometry_msgs::msg::Pose & map_ if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( map_pose, status_->getBoundingBox(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { requestAcquirePosition(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Goal of the vehicle entity should be on lane."); @@ -202,7 +202,7 @@ void VehicleEntity::requestAssignRoute(const std::vectorgetBoundingBox(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { route.emplace_back(canonicalized_lanelet_pose.value()); } else { THROW_SEMANTIC_ERROR("Waypoint of vehicle entity should be on lane."); @@ -221,7 +221,7 @@ auto VehicleEntity::requestFollowTrajectory( if ( const auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( vertex.position, status_->getBoundingBox(), false, - getDefaultMatchingDistanceForLaneletPoseCalculation(), hdmap_utils_ptr_)) { + getDefaultMatchingDistanceForLaneletPoseCalculation())) { waypoints.emplace_back(canonicalized_lanelet_pose.value()); } else { /// @todo such a protection most likely makes sense, but test scenario diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index c6cb0554b23..8a4c5043465 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -51,12 +51,17 @@ #include #include #include +#include +#include +#include #include #include #include namespace hdmap_utils { +using namespace traffic_simulator::lanelet_wrapper; + HdMapUtils::HdMapUtils( const boost::filesystem::path & lanelet2_map_path, const geographic_msgs::msg::GeoPoint &) { @@ -80,136 +85,14 @@ HdMapUtils::HdMapUtils( overwriteLaneletsCenterline(); } -auto HdMapUtils::getAllCanonicalizedLaneletPoses( - const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const - -> std::vector -{ - /// @note Define lambda functions for canonicalizing previous/next lanelet. - const auto canonicalize_to_previous_lanelet = - [this](const auto & lanelet_pose) -> std::vector { - if (const auto ids = getPreviousLaneletIds(lanelet_pose.lanelet_id); !ids.empty()) { - std::vector canonicalized_all; - for (const auto id : ids) { - const auto lanelet_pose_tmp = traffic_simulator::helper::constructLaneletPose( - id, lanelet_pose.s + getLaneletLength(id), lanelet_pose.offset); - if (const auto canonicalized_lanelet_poses = - getAllCanonicalizedLaneletPoses(lanelet_pose_tmp); - canonicalized_lanelet_poses.empty()) { - canonicalized_all.emplace_back(lanelet_pose_tmp); - } else { - std::copy( - canonicalized_lanelet_poses.begin(), canonicalized_lanelet_poses.end(), - std::back_inserter(canonicalized_all)); - } - } - return canonicalized_all; - } else { - return {}; - } - }; - const auto canonicalize_to_next_lanelet = - [this](const auto & lanelet_pose) -> std::vector { - if (const auto ids = getNextLaneletIds(lanelet_pose.lanelet_id); !ids.empty()) { - std::vector canonicalized_all; - for (const auto id : ids) { - const auto lanelet_pose_tmp = traffic_simulator::helper::constructLaneletPose( - id, lanelet_pose.s - getLaneletLength(lanelet_pose.lanelet_id), lanelet_pose.offset); - if (const auto canonicalized_lanelet_poses = - getAllCanonicalizedLaneletPoses(lanelet_pose_tmp); - canonicalized_lanelet_poses.empty()) { - canonicalized_all.emplace_back(lanelet_pose_tmp); - } else { - std::copy( - canonicalized_lanelet_poses.begin(), canonicalized_lanelet_poses.end(), - std::back_inserter(canonicalized_all)); - } - } - return canonicalized_all; - } else { - return {}; - } - }; - - /// @note If s value under 0, it means this pose is on the previous lanelet. - if (lanelet_pose.s < 0) { - return canonicalize_to_previous_lanelet(lanelet_pose); - } - /// @note If s value overs it's lanelet length, it means this pose is on the next lanelet. - else if (lanelet_pose.s > (getLaneletLength(lanelet_pose.lanelet_id))) { - return canonicalize_to_next_lanelet(lanelet_pose); - } - /// @note If s value is in range [0,length_of_the_lanelet], return lanelet_pose. - else { - return {lanelet_pose}; - } -} - -// If route is not specified, the lanelet_id with the lowest array index is used as a candidate for -// canonicalize destination. -auto HdMapUtils::canonicalizeLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const - -> std::tuple, std::optional> -{ - auto canonicalized = lanelet_pose; - while (canonicalized.s < 0) { - if (const auto ids = getPreviousLaneletIds(canonicalized.lanelet_id); ids.empty()) { - return {std::nullopt, canonicalized.lanelet_id}; - } else { - canonicalized.s += getLaneletLength(ids[0]); - canonicalized.lanelet_id = ids[0]; - } - } - while (canonicalized.s > getLaneletLength(canonicalized.lanelet_id)) { - if (const auto ids = getNextLaneletIds(canonicalized.lanelet_id); ids.empty()) { - return {std::nullopt, canonicalized.lanelet_id}; - } else { - canonicalized.s -= getLaneletLength(canonicalized.lanelet_id); - canonicalized.lanelet_id = ids[0]; - } - } - return {canonicalized, std::nullopt}; -} - -auto HdMapUtils::canonicalizeLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, - const lanelet::Ids & route_lanelets) const - -> std::tuple, std::optional> -{ - auto canonicalized = lanelet_pose; - while (canonicalized.s < 0) { - // When canonicalizing to backward lanelet_id, do not consider route - if (const auto ids = getPreviousLaneletIds(canonicalized.lanelet_id); ids.empty()) { - return {std::nullopt, canonicalized.lanelet_id}; - } else { - canonicalized.s += getLaneletLength(ids[0]); - canonicalized.lanelet_id = ids[0]; - } - } - while (canonicalized.s > getLaneletLength(canonicalized.lanelet_id)) { - bool next_lanelet_found = false; - // When canonicalizing to forward lanelet_id, consider route - for (const auto id : getNextLaneletIds(canonicalized.lanelet_id)) { - if (std::any_of(route_lanelets.begin(), route_lanelets.end(), [id](auto id_on_route) { - return id == id_on_route; - })) { - canonicalized.s -= getLaneletLength(canonicalized.lanelet_id); - canonicalized.lanelet_id = id; - next_lanelet_found = true; - } - } - if (!next_lanelet_found) { - return {std::nullopt, canonicalized.lanelet_id}; - } - } - return {canonicalized, std::nullopt}; -} - auto HdMapUtils::countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, const traffic_simulator_msgs::msg::LaneletPose & to, const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional> { + /// @note a lane change considers the lanes in the same direction as the original, so ignore the lanes in the opposite direction + constexpr bool include_opposite_direction{false}; const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); if (route.empty()) { return std::nullopt; @@ -218,18 +101,18 @@ auto HdMapUtils::countLaneChanges( for (std::size_t i = 1; i < route.size(); ++i) { const auto & previous = route[i - 1]; const auto & current = route[i]; - - if (auto followings = getNextLaneletIds(previous, routing_configuration.routing_graph_type); - std::find(followings.begin(), followings.end(), current) == followings.end()) { - if (auto lefts = - getLeftLaneletIds(previous, routing_configuration.routing_graph_type, false); - std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { - lane_changes.first++; - } else if (auto rights = - getRightLaneletIds(previous, routing_configuration.routing_graph_type, false); - std::find(rights.begin(), rights.end(), current) != rights.end()) { - lane_changes.second++; - } + if (const auto followings = + lanelet_map::nextLaneletIds(previous, routing_configuration.routing_graph_type); + std::find(followings.begin(), followings.end(), current) != followings.end()) { + continue; + } else if (const auto lefts = pose::leftLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { + lane_changes.first++; + } else if (const auto rights = pose::rightLaneletIds( + previous, routing_configuration.routing_graph_type, include_opposite_direction); + std::find(rights.begin(), rights.end(), current) != rights.end()) { + lane_changes.second++; } } return lane_changes; @@ -337,7 +220,7 @@ auto HdMapUtils::getNearbyLaneletIds( auto HdMapUtils::getAltitude(const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) const -> double { - return toMapPose(lanelet_pose).pose.position.z; + return pose::toMapPose(lanelet_pose).pose.position.z; } auto HdMapUtils::getCollisionPointInLaneCoordinate( @@ -435,12 +318,12 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( bool on_traj = false; double rest_distance = forward_distance; for (auto id_itr = lanelet_ids.begin(); id_itr != lanelet_ids.end(); id_itr++) { - double l = getLaneletLength(*id_itr); + double l = lanelet_map::laneletLength(*id_itr); if (on_traj) { if (rest_distance < l) { for (double s_val = 0; s_val < rest_distance; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0)) .pose.position); } break; @@ -448,7 +331,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( rest_distance = rest_distance - l; for (double s_val = 0; s_val < l; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0.0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(*id_itr, s_val, 0.0)) .pose.position); } continue; @@ -459,7 +342,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( if ((s + forward_distance) < l) { for (double s_val = s; s_val < s + forward_distance; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) .pose.position); } break; @@ -467,7 +350,7 @@ auto HdMapUtils::clipTrajectoryFromLaneletIds( rest_distance = rest_distance - (l - s); for (double s_val = s; s_val < l; s_val = s_val + 1.0) { ret.emplace_back( - toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) + pose::toMapPose(traffic_simulator::helper::constructLaneletPose(lanelet_id, s_val, 0.0)) .pose.position); } continue; @@ -525,199 +408,6 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan return lanelet::BasicPoint2d{point.x, point.y}; } -auto HdMapUtils::matchToLane( - const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, const double reduction_ratio, - const traffic_simulator::RoutingGraphType type) const -> std::optional -{ - lanelet::matching::Object2d obj; - obj.pose.translation() = toPoint2d(pose.position); - obj.pose.linear() = - Eigen::Rotation2D(math::geometry::convertQuaternionToEulerAngle(pose.orientation).z) - .matrix(); - obj.absoluteHull = absoluteHull( - lanelet::matching::Hull2d{ - lanelet::BasicPoint2d{ - bbox.center.x + bbox.dimensions.x * 0.5 * reduction_ratio, - bbox.center.y + bbox.dimensions.y * 0.5 * reduction_ratio}, - lanelet::BasicPoint2d{ - bbox.center.x - bbox.dimensions.x * 0.5 * reduction_ratio, - bbox.center.y - bbox.dimensions.y * 0.5 * reduction_ratio}}, - obj.pose); - auto matches = - lanelet::matching::getDeterministicMatches(*lanelet_map_ptr_, obj, matching_distance); - if (!include_crosswalk) { - matches = lanelet::matching::removeNonRuleCompliantMatches( - matches, routing_graphs_->traffic_rule(type)); - } - if (matches.empty()) { - return std::nullopt; - } - std::vector> id_and_distance; - for (const auto & match : matches) { - if (const auto lanelet_pose = toLaneletPose(pose, match.lanelet.id(), matching_distance)) { - id_and_distance.emplace_back(lanelet_pose->lanelet_id, lanelet_pose->offset); - } - } - if (id_and_distance.empty()) { - return std::nullopt; - } - const auto min_id_and_distance = std::min_element( - id_and_distance.begin(), id_and_distance.end(), - [](auto const & lhs, auto const & rhs) { return lhs.second < rhs.second; }); - return min_id_and_distance->first; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const bool include_crosswalk, - const double matching_distance) const -> std::optional -{ - const auto lanelet_ids = getNearbyLaneletIds(pose.position, 0.1, include_crosswalk); - if (lanelet_ids.empty()) { - return std::nullopt; - } - for (const auto & id : lanelet_ids) { - const auto lanelet_pose = toLaneletPose(pose, id, matching_distance); - if (lanelet_pose) { - return lanelet_pose; - } - } - return std::nullopt; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id, - const double matching_distance) const -> std::optional -{ - const auto spline = getCenterPointsSpline(lanelet_id); - const auto s = spline->getSValue(pose, matching_distance); - if (!s) { - return std::nullopt; - } - auto pose_on_centerline = spline->getPose(s.value()); - - if (!isAltitudeMatching(pose.position.z, pose_on_centerline.position.z)) { - return std::nullopt; - } - - auto rpy = math::geometry::convertQuaternionToEulerAngle( - math::geometry::getRotation(pose_on_centerline.orientation, pose.orientation)); - double offset = std::sqrt(spline->getSquaredDistanceIn2D(pose.position, s.value())); - /** - * @note Hard coded parameter - */ - double yaw_threshold = 0.25; - if (M_PI * yaw_threshold < std::fabs(rpy.z) && std::fabs(rpy.z) < M_PI * (1 - yaw_threshold)) { - return std::nullopt; - } - double inner_prod = math::geometry::innerProduct( - spline->getNormalVector(s.value()), spline->getSquaredDistanceVector(pose.position, s.value())); - if (inner_prod < 0) { - offset = offset * -1; - } - traffic_simulator_msgs::msg::LaneletPose lanelet_pose; - lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = s.value(); - lanelet_pose.offset = offset; - lanelet_pose.rpy = rpy; - return lanelet_pose; -} - -auto HdMapUtils::isAltitudeMatching( - const double current_altitude, const double target_altitude) const -> bool -{ - /* - Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the - entity's Z-position is always relative to its base. This eliminates the - need to dynamically adjust the threshold based on the entity's dimensions, - ensuring consistent altitude matching regardless of the entity type. - - The position of the entity is defined relative to its base, typically - the center of the rear axle projected onto the ground in the case of vehicles. - - There is no technical basis for this value; it was determined based on - experiments. - */ - static constexpr double altitude_threshold = 1.0; - return std::abs(current_altitude - target_altitude) <= altitude_threshold; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const lanelet::Ids & lanelet_ids, - const double matching_distance) const -> std::optional -{ - for (const auto id : lanelet_ids) { - if (const auto lanelet_pose = toLaneletPose(pose, id, matching_distance); lanelet_pose) { - return lanelet_pose.value(); - } - } - return std::nullopt; -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Point & position, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, - const traffic_simulator::RoutingGraphType type) const - -> std::optional -{ - return toLaneletPose( - geometry_msgs::build().position(position).orientation( - geometry_msgs::build().x(0).y(0).z(0).w(1)), - bbox, include_crosswalk, matching_distance, type); -} - -auto HdMapUtils::toLaneletPose( - const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, - const traffic_simulator::RoutingGraphType type) const - -> std::optional -{ - const auto lanelet_id = matchToLane( - pose, bbox, include_crosswalk, matching_distance, DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, type); - if (!lanelet_id) { - return toLaneletPose(pose, include_crosswalk, matching_distance); - } - const auto pose_in_target_lanelet = toLaneletPose(pose, lanelet_id.value(), matching_distance); - if (pose_in_target_lanelet) { - return pose_in_target_lanelet; - } - const auto previous = getPreviousLaneletIds(lanelet_id.value(), type); - for (const auto id : previous) { - const auto pose_in_previous = toLaneletPose(pose, id, matching_distance); - if (pose_in_previous) { - return pose_in_previous; - } - } - const auto next = getNextLaneletIds(lanelet_id.value(), type); - for (const auto id : previous) { - const auto pose_in_next = toLaneletPose(pose, id, matching_distance); - if (pose_in_next) { - return pose_in_next; - } - } - return toLaneletPose(pose, include_crosswalk, matching_distance); -} - -auto HdMapUtils::toLaneletPoses( - const geometry_msgs::msg::Pose & pose, const lanelet::Id lanelet_id, - const double matching_distance, const bool include_opposite_direction, - const traffic_simulator::RoutingGraphType type) const - -> std::vector -{ - std::vector ret; - std::vector lanelet_ids = {lanelet_id}; - lanelet_ids += getLeftLaneletIds(lanelet_id, type, include_opposite_direction); - lanelet_ids += getRightLaneletIds(lanelet_id, type, include_opposite_direction); - lanelet_ids += getPreviousLaneletIds(lanelet_ids, type); - lanelet_ids += getNextLaneletIds(lanelet_ids, type); - for (const auto & id : sortAndUnique(lanelet_ids)) { - if (const auto & lanelet_pose = toLaneletPose(pose, id, matching_distance)) { - ret.emplace_back(lanelet_pose.value()); - } - } - return ret; -} - auto HdMapUtils::getClosestLaneletId( const geometry_msgs::msg::Pose & pose, const double distance_thresh, const bool include_crosswalk) const -> std::optional @@ -824,13 +514,14 @@ auto HdMapUtils::getPreviousLanelets( while (total_distance < backward_horizon) { const auto & reference_lanelet_id = previous_lanelets_ids.back(); if (const auto straight_lanelet_ids = - getPreviousLaneletIds(reference_lanelet_id, "straight", type); + lanelet_map::previousLaneletIds(reference_lanelet_id, "straight", type); not straight_lanelet_ids.empty()) { - total_distance = total_distance + getLaneletLength(straight_lanelet_ids[0]); + total_distance = total_distance + lanelet_map::laneletLength(straight_lanelet_ids[0]); previous_lanelets_ids.push_back(straight_lanelet_ids[0]); - } else if (auto non_straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id, type); + } else if (auto non_straight_lanelet_ids = + lanelet_map::previousLaneletIds(reference_lanelet_id, type); not non_straight_lanelet_ids.empty()) { - total_distance = total_distance + getLaneletLength(non_straight_lanelet_ids[0]); + total_distance = total_distance + lanelet_map::laneletLength(non_straight_lanelet_ids[0]); previous_lanelets_ids.push_back(non_straight_lanelet_ids[0]); } else { break; @@ -851,7 +542,7 @@ auto HdMapUtils::getFollowingLanelets( { const auto is_following_lanelet = [this, type](const auto & current_lanelet, const auto & candidate_lanelet) { - const auto next_ids = getNextLaneletIds(current_lanelet, type); + const auto next_ids = lanelet_map::nextLaneletIds(current_lanelet, type); return std::find(next_ids.cbegin(), next_ids.cend(), candidate_lanelet) != next_ids.cend(); }; @@ -873,7 +564,7 @@ auto HdMapUtils::getFollowingLanelets( candidate_lanelet_id + " is not the follower of lanelet " + previous_lanelet); } following_lanelets_ids.push_back(candidate_lanelet_id); - total_distance += getLaneletLength(candidate_lanelet_id); + total_distance += lanelet_map::laneletLength(candidate_lanelet_id); if (total_distance > horizon) { break; } @@ -907,14 +598,15 @@ auto HdMapUtils::getFollowingLanelets( } lanelet::Id end_lanelet_id = lanelet_id; while (total_distance < distance) { - if (const auto straight_ids = getNextLaneletIds(end_lanelet_id, "straight", type); + if (const auto straight_ids = lanelet_map::nextLaneletIds(end_lanelet_id, "straight", type); !straight_ids.empty()) { - total_distance = total_distance + getLaneletLength(straight_ids[0]); + total_distance = total_distance + lanelet_map::laneletLength(straight_ids[0]); ret.push_back(straight_ids[0]); end_lanelet_id = straight_ids[0]; continue; - } else if (const auto ids = getNextLaneletIds(end_lanelet_id, type); ids.size() != 0) { - total_distance = total_distance + getLaneletLength(ids[0]); + } else if (const auto ids = lanelet_map::nextLaneletIds(end_lanelet_id, type); + ids.size() != 0) { + total_distance = total_distance + lanelet_map::laneletLength(ids[0]); ret.push_back(ids[0]); end_lanelet_id = ids[0]; continue; @@ -993,112 +685,6 @@ auto HdMapUtils::getCenterPoints(const lanelet::Id lanelet_id) const return ret; } -auto HdMapUtils::getLaneletLength(const lanelet::Id lanelet_id) const -> double -{ - if (lanelet_length_cache_.exists(lanelet_id)) { - return lanelet_length_cache_.getLength(lanelet_id); - } - double ret = lanelet::utils::getLaneletLength2d(lanelet_map_ptr_->laneletLayer.get(lanelet_id)); - lanelet_length_cache_.appendData(lanelet_id, ret); - return ret; -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->previous(lanelet)) { - ids.push_back(llt.id()); - } - return ids; -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id, type); - } - return sortAndUnique(ids); -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->previous(lanelet)) { - if (llt.attributeOr("turn_direction", "else") == turn_direction) { - ids.push_back(llt.id()); - } - } - return ids; -} - -auto HdMapUtils::getPreviousLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getPreviousLaneletIds(id, turn_direction, type); - } - return sortAndUnique(ids); -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->following(lanelet)) { - ids.push_back(llt.id()); - } - return ids; -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const - -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id, type); - } - return sortAndUnique(ids); -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Id lanelet_id, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : routing_graphs_->routing_graph(type)->following(lanelet)) { - if (llt.attributeOr("turn_direction", "else") == turn_direction) { - ids.push_back(llt.id()); - } - } - return ids; -} - -auto HdMapUtils::getNextLaneletIds( - const lanelet::Ids & lanelet_ids, const std::string & turn_direction, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids -{ - lanelet::Ids ids; - for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id, turn_direction, type); - } - return sortAndUnique(ids); -} - auto HdMapUtils::getTrafficLightIds() const -> lanelet::Ids { using namespace lanelet::utils::query; @@ -1154,44 +740,6 @@ auto HdMapUtils::getTrafficLightBulbPosition( return std::nullopt; } -auto HdMapUtils::getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from_pose, const double along, - const traffic_simulator::RoutingGraphType type) const -> traffic_simulator_msgs::msg::LaneletPose -{ - traffic_simulator_msgs::msg::LaneletPose along_pose = from_pose; - along_pose.s = along_pose.s + along; - if (along_pose.s >= 0) { - while (along_pose.s >= getLaneletLength(along_pose.lanelet_id)) { - auto next_ids = getNextLaneletIds(along_pose.lanelet_id, "straight", type); - if (next_ids.empty()) { - next_ids = getNextLaneletIds(along_pose.lanelet_id, type); - if (next_ids.empty()) { - THROW_SEMANTIC_ERROR( - "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", - from_pose.s + along, "), next lanelet of id = ", along_pose.lanelet_id, "is empty."); - } - } - along_pose.s = along_pose.s - getLaneletLength(along_pose.lanelet_id); - along_pose.lanelet_id = next_ids[0]; - } - } else { - while (along_pose.s < 0) { - auto previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, "straight", type); - if (previous_ids.empty()) { - previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, type); - if (previous_ids.empty()) { - THROW_SEMANTIC_ERROR( - "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", - from_pose.s + along, "), next lanelet of id = ", along_pose.lanelet_id, "is empty."); - } - } - along_pose.s = along_pose.s + getLaneletLength(previous_ids[0]); - along_pose.lanelet_id = previous_ids[0]; - } - } - return along_pose; -} - auto HdMapUtils::getLeftBound(const lanelet::Id lanelet_id) const -> std::vector { @@ -1204,33 +752,6 @@ auto HdMapUtils::getRightBound(const lanelet::Id lanelet_id) const return toPolygon(lanelet_map_ptr_->laneletLayer.get(lanelet_id).rightBound()); } -auto HdMapUtils::getLeftLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type, - const bool include_opposite_direction) const -> lanelet::Ids -{ - if (include_opposite_direction) { - throw common::Error( - "HdMapUtils::getLeftLaneletIds with include_opposite_direction=true is not implemented yet."); - } else { - return getLaneletIds( - routing_graphs_->routing_graph(type)->lefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); - } -} - -auto HdMapUtils::getRightLaneletIds( - const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type, - const bool include_opposite_direction) const -> lanelet::Ids -{ - if (include_opposite_direction) { - throw common::Error( - "HdMapUtils::getRightLaneletIds with include_opposite_direction=true is not implemented " - "yet."); - } else { - return getLaneletIds( - routing_graphs_->routing_graph(type)->rights(lanelet_map_ptr_->laneletLayer.get(lanelet_id))); - } -} - auto HdMapUtils::getLaneChangeTrajectory( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator::lane_change::Parameter & lane_change_parameter) const @@ -1255,13 +776,13 @@ auto HdMapUtils::getLaneChangeTrajectory( traffic_simulator::lane_change::Parameter::default_lanechange_distance; break; } - const auto along_pose = getAlongLaneletPose(from_pose, longitudinal_distance); + const auto along_pose = pose::alongLaneletPose(from_pose, longitudinal_distance); // clang-format off const auto left_point = - toMapPose(traffic_simulator::helper::constructLaneletPose( + pose::toMapPose(traffic_simulator::helper::constructLaneletPose( along_pose.lanelet_id, along_pose.s, along_pose.offset + 5.0)).pose.position; const auto right_point = - toMapPose(traffic_simulator::helper::constructLaneletPose( + pose::toMapPose(traffic_simulator::helper::constructLaneletPose( along_pose.lanelet_id, along_pose.s, along_pose.offset - 5.0)).pose.position; // clang-format on const auto collision_point = getCenterPointsSpline(lane_change_parameter.target.lanelet_id) @@ -1272,15 +793,15 @@ auto HdMapUtils::getLaneChangeTrajectory( const auto to_pose = traffic_simulator::helper::constructLaneletPose( lane_change_parameter.target.lanelet_id, collision_point.value(), lane_change_parameter.target.offset); - const auto goal_pose_in_map = toMapPose(to_pose).pose; - const auto from_pose_in_map = toMapPose(from_pose).pose; + const auto goal_pose_in_map = pose::toMapPose(to_pose).pose; + const auto from_pose_in_map = pose::toMapPose(from_pose).pose; double start_to_goal_distance = std::sqrt( std::pow(from_pose_in_map.position.x - goal_pose_in_map.position.x, 2) + std::pow(from_pose_in_map.position.y - goal_pose_in_map.position.y, 2) + std::pow(from_pose_in_map.position.z - goal_pose_in_map.position.z, 2)); auto traj = getLaneChangeTrajectory( - toMapPose(from_pose).pose, to_pose, lane_change_parameter.trajectory_shape, + pose::toMapPose(from_pose).pose, to_pose, lane_change_parameter.trajectory_shape, start_to_goal_distance * 0.5); return std::make_pair(traj, collision_point.value()); } @@ -1292,12 +813,12 @@ auto HdMapUtils::getLaneChangeTrajectory( const double forward_distance_threshold) const -> std::optional> { - double to_length = getLaneletLength(lane_change_parameter.target.lanelet_id); + double to_length = lanelet_map::laneletLength(lane_change_parameter.target.lanelet_id); std::vector evaluation, target_s; std::vector curves; for (double to_s = 0; to_s < to_length; to_s = to_s + 1.0) { - auto goal_pose = toMapPose(traffic_simulator::helper::constructLaneletPose( + auto goal_pose = pose::toMapPose(traffic_simulator::helper::constructLaneletPose( lane_change_parameter.target.lanelet_id, to_s)); if ( math::geometry::getRelativePose(from_pose, goal_pose.pose).position.x <= @@ -1336,7 +857,7 @@ auto HdMapUtils::getLaneChangeTrajectory( { geometry_msgs::msg::Vector3 start_vec; geometry_msgs::msg::Vector3 to_vec; - geometry_msgs::msg::Pose goal_pose = toMapPose(to_pose).pose; + geometry_msgs::msg::Pose goal_pose = pose::toMapPose(to_pose).pose; double tangent_vector_size_in_curve = 0.0; switch (trajectory_shape) { case traffic_simulator::lane_change::TrajectoryShape::CUBIC: @@ -1398,38 +919,6 @@ auto HdMapUtils::toMapPoints(const lanelet::Id lanelet_id, const std::vector geometry_msgs::msg::PoseStamped -{ - using math::geometry::operator*; - using math::geometry::operator+=; - if ( - const auto pose = std::get>( - canonicalizeLaneletPose(lanelet_pose))) { - geometry_msgs::msg::PoseStamped ret; - ret.header.frame_id = "map"; - const auto spline = getCenterPointsSpline(pose->lanelet_id); - ret.pose = spline->getPose(pose->s); - const auto normal_vec = spline->getNormalVector(pose->s); - const auto diff = math::geometry::normalize(normal_vec) * pose->offset; //this - ret.pose.position += diff; - const auto tangent_vec = spline->getTangentVector(pose->s); - geometry_msgs::msg::Vector3 rpy; - rpy.x = 0.0; - rpy.y = fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0; - rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); - ret.pose.orientation = math::geometry::convertEulerAngleToQuaternion(rpy) * - math::geometry::convertEulerAngleToQuaternion(pose->rpy); - return ret; - } else { - THROW_SEMANTIC_ERROR( - "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, - ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, - ",rpy.z=", lanelet_pose.rpy.z, ") is invalid, please check lanelet length and connection."); - } -} - auto HdMapUtils::getTangentVector(const lanelet::Id lanelet_id, const double s) const -> std::optional { @@ -1458,7 +947,8 @@ auto HdMapUtils::getLateralDistance( if (routing_configuration.allow_lane_change) { double lateral_distance_by_lane_change = 0.0; for (unsigned int i = 0; i < route.size() - 1; i++) { - auto next_lanelet_ids = getNextLaneletIds(route[i], routing_configuration.routing_graph_type); + auto next_lanelet_ids = + lanelet_map::nextLaneletIds(route[i], routing_configuration.routing_graph_type); if (auto next_lanelet = std::find_if( next_lanelet_ids.begin(), next_lanelet_ids.end(), [&route, i](const lanelet::Id & id) { return id == route[i + 1]; }); @@ -1470,14 +960,14 @@ auto HdMapUtils::getLateralDistance( if ( auto next_lanelet_origin_from_current_lanelet = - toLaneletPose(toMapPose(next_lanelet_pose).pose, route[i], 10.0)) { + pose::toLaneletPose(pose::toMapPose(next_lanelet_pose).pose, route[i], 10.0)) { lateral_distance_by_lane_change += next_lanelet_origin_from_current_lanelet->offset; } else { traffic_simulator_msgs::msg::LaneletPose current_lanelet_pose = next_lanelet_pose; current_lanelet_pose.lanelet_id = route[i]; if ( auto current_lanelet_origin_from_next_lanelet = - toLaneletPose(toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) { + pose::toLaneletPose(pose::toMapPose(current_lanelet_pose).pose, route[i + 1], 10.0)) { lateral_distance_by_lane_change -= current_lanelet_origin_from_next_lanelet->offset; } else { return std::nullopt; @@ -1497,11 +987,11 @@ auto HdMapUtils::getLongitudinalDistance( const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional { - const auto is_lane_change_required = [this, routing_configuration]( + const auto is_lane_change_required = [routing_configuration]( const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool { const auto next_lanelet_ids = - getNextLaneletIds(current_lanelet, routing_configuration.routing_graph_type); + lanelet_map::nextLaneletIds(current_lanelet, routing_configuration.routing_graph_type); return std::none_of( next_lanelet_ids.cbegin(), next_lanelet_ids.cend(), [next_lanelet](const lanelet::Id id) { return id == next_lanelet; }); @@ -1548,7 +1038,7 @@ auto HdMapUtils::getLongitudinalDistance( return std::nullopt; } } else { - accumulated_distance += getLaneletLength(route[i - 1UL]); + accumulated_distance += lanelet_map::laneletLength(route[i - 1UL]); } } // subtract the distance already traveled on the first lanelet: from_pose.s @@ -2196,18 +1686,6 @@ auto HdMapUtils::toPolygon(const lanelet::ConstLineString3d & line_string) const return ret; } -auto HdMapUtils::getLaneletAltitude( - const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, - const double matching_distance) const -> std::optional -{ - if (const auto spline = getCenterPointsSpline(lanelet_id)) { - if (const auto s = spline->getSValue(pose, matching_distance)) { - return spline->getPoint(s.value()).z; - } - } - return std::nullopt; -} - HdMapUtils::RoutingGraphs::RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_map) { vehicle.rules = lanelet::traffic_rules::TrafficRulesFactory::create( diff --git a/simulation/traffic_simulator/src/helper/helper.cpp b/simulation/traffic_simulator/src/helper/helper.cpp index ccd672cdb94..b36e3aacfb1 100644 --- a/simulation/traffic_simulator/src/helper/helper.cpp +++ b/simulation/traffic_simulator/src/helper/helper.cpp @@ -51,13 +51,12 @@ LaneletPose constructLaneletPose( } auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose + lanelet::Id lanelet_id, double s, double offset, double roll, double pitch, double yaw) + -> CanonicalizedLaneletPose { if ( - auto canonicalized_lanelet_pose = pose::canonicalize( - traffic_simulator::helper::constructLaneletPose(lanelet_id, s, offset, roll, pitch, yaw), - hdmap_utils_ptr)) { + auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(lanelet_id, s, offset, roll, pitch, yaw))) { return canonicalized_lanelet_pose.value(); } else { THROW_SEMANTIC_ERROR( @@ -67,11 +66,10 @@ auto constructCanonicalizedLaneletPose( } } -auto constructCanonicalizedLaneletPose( - lanelet::Id lanelet_id, double s, double offset, - const std::shared_ptr & hdmap_utils_ptr) -> CanonicalizedLaneletPose +auto constructCanonicalizedLaneletPose(lanelet::Id lanelet_id, double s, double offset) + -> CanonicalizedLaneletPose { - return constructCanonicalizedLaneletPose(lanelet_id, s, offset, 0, 0, 0, hdmap_utils_ptr); + return constructCanonicalizedLaneletPose(lanelet_id, s, offset, 0, 0, 0); } geometry_msgs::msg::Vector3 constructRPY(double roll, double pitch, double yaw) @@ -189,9 +187,10 @@ const simulation_api_schema::LidarConfiguration constructLidarConfiguration( } // namespace helper } // namespace traffic_simulator -std::ostream & operator<<(std::ostream & os, const traffic_simulator::LaneletPose & ll_pose) +std::ostream & operator<<( + std::ostream & os, const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose) { - os << "lanelet id : " << ll_pose.lanelet_id << "\ns : " << ll_pose.s; + os << "lanelet id : " << lanelet_pose.lanelet_id << "\ns : " << lanelet_pose.s; return os; } diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp new file mode 100644 index 00000000000..59295dd6146 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_loader.cpp @@ -0,0 +1,160 @@ +// 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 + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +auto LaneletLoader::load(const std::filesystem::path & lanelet_map_path) -> lanelet::LaneletMapPtr +{ + lanelet::projection::MGRSProjector mgrs_projector; + lanelet::ErrorMessages lanelet_errors; + lanelet::LaneletMapPtr lanelet_map_ptr = + lanelet::load(lanelet_map_path.string(), mgrs_projector, &lanelet_errors); + + if (!lanelet_errors.empty()) { + std::stringstream ss; + ss << "Failed to load lanelet map, errors:\n"; + for (const auto & error : lanelet_errors) { + ss << " - " << error << "\n"; + } + THROW_SIMULATION_ERROR(ss.str()); + } + + if (lanelet_map_ptr->laneletLayer.empty()) { + THROW_SIMULATION_ERROR("Lanelet layer is empty!"); + } + + overwriteLaneletsCenterline(lanelet_map_ptr); + return lanelet_map_ptr; +} + +auto LaneletLoader::overwriteLaneletsCenterline(lanelet::LaneletMapPtr lanelet_map_ptr) -> void +{ + constexpr double resolution{2.0}; + + auto generateFineCenterline = + [&](const lanelet::ConstLanelet & lanelet_obj) -> lanelet::LineString3d { + /// @note Get length of longer border + const auto left_length = + static_cast(lanelet::geometry::length(lanelet_obj.leftBound())); + const auto right_length = + static_cast(lanelet::geometry::length(lanelet_obj.rightBound())); + const auto longer_distance = (left_length > right_length) ? left_length : right_length; + const auto num_segments = std::max(static_cast(ceil(longer_distance / resolution)), 1); + + /// @note Resample points + const auto left_points = resamplePoints(lanelet_obj.leftBound(), num_segments); + const auto right_points = resamplePoints(lanelet_obj.rightBound(), num_segments); + + /// @note Create centerline + lanelet::LineString3d centerline(lanelet::utils::getId()); + for (size_t i = 0; i < static_cast(num_segments + 1); i++) { + /// @note Add ID for the average point of left and right + const auto center_basic_point = (right_points.at(i) + left_points.at(i)) / 2.0; + const lanelet::Point3d center_point( + lanelet::utils::getId(), center_basic_point.x(), center_basic_point.y(), + center_basic_point.z()); + centerline.push_back(center_point); + } + return centerline; + }; + + for (auto & lanelet_obj : lanelet_map_ptr->laneletLayer) { + if (!lanelet_obj.hasCustomCenterline()) { + lanelet_obj.setCenterline(generateFineCenterline(lanelet_obj)); + } + } +} + +auto LaneletLoader::resamplePoints( + const lanelet::ConstLineString3d & line_string, const std::int32_t num_segments) + -> lanelet::BasicPoints3d +{ + const auto accumulated_lengths = calculateAccumulatedLengths(line_string); + + auto findNearestIndexPair = + [&](const double target_length) -> std::pair { + const auto N = accumulated_lengths.size(); + if (target_length < accumulated_lengths.at(1)) { + return std::make_pair(0, 1); + } else if (target_length > accumulated_lengths.at(N - 2)) { + return std::make_pair(N - 2, N - 1); + } else { + for (size_t i = 1; i < N; ++i) { + if ( + accumulated_lengths.at(i - 1) <= target_length && + target_length <= accumulated_lengths.at(i)) { + return std::make_pair(i - 1, i); + } + } + } + THROW_SEMANTIC_ERROR("findNearestIndexPair(): No nearest point found."); + }; + + /// @note Create each segment + lanelet::BasicPoints3d resampled_points; + for (auto i = 0; i <= num_segments; ++i) { + /// @note Find two nearest points + const double target_length = (static_cast(i) / num_segments) * + static_cast(lanelet::geometry::length(line_string)); + const auto [first_index, second_index] = findNearestIndexPair(target_length); + + /// @note Apply linear interpolation + const lanelet::BasicPoint3d back_point = line_string[first_index]; + const lanelet::BasicPoint3d front_point = line_string[second_index]; + const auto direction_vector = (front_point - back_point); + + const auto back_length = accumulated_lengths.at(first_index); + const auto front_length = accumulated_lengths.at(second_index); + const auto segment_length = front_length - back_length; + const auto target_point = + back_point + (direction_vector * (target_length - back_length) / segment_length); + + /// @note Add to list + resampled_points.emplace_back(target_point); + } + return resampled_points; +} + +auto LaneletLoader::calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) + -> std::vector +{ + auto calculateSegmentDistances = + [](const lanelet::ConstLineString3d & line) -> std::vector { + std::vector segment_distances; + segment_distances.reserve(line.size() - 1); + for (size_t i = 1; i < line.size(); ++i) { + segment_distances.push_back(lanelet::geometry::distance(line[i], line[i - 1])); + } + return segment_distances; + }; + + const auto segment_distances = calculateSegmentDistances(line_string); + std::vector accumulated_lengths{0}; + accumulated_lengths.reserve(segment_distances.size() + 1); + std::partial_sum( + std::begin(segment_distances), std::end(segment_distances), + std::back_inserter(accumulated_lengths)); + return accumulated_lengths; +} +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp new file mode 100644 index 00000000000..7a8d673cd5f --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp @@ -0,0 +1,240 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace lanelet_map +{ +auto isInLanelet(const lanelet::Id lanelet_id, const double lanelet_pose_s) -> bool +{ + return 0 <= lanelet_pose_s and lanelet_pose_s <= laneletLength(lanelet_id); +} + +auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool +{ + return lanelet::geometry::inside( + LaneletWrapper::map()->laneletLayer.get(lanelet_id), lanelet::BasicPoint2d(point.x, point.y)); +} + +auto laneletLength(const lanelet::Id lanelet_id) -> double +{ + return LaneletWrapper::laneletLengthCache().getLength(lanelet_id, LaneletWrapper::map()); +} + +auto laneletAltitude( + const lanelet::Id & lanelet_id, const geometry_msgs::msg::Pose & pose, + const double matching_distance) -> std::optional +{ + if (const auto spline = centerPointsSpline(lanelet_id)) { + if (const auto s = spline->getSValue(pose, matching_distance)) { + return spline->getPoint(s.value()).z; + } + } + return std::nullopt; +} + +auto laneletIds() -> lanelet::Ids +{ + lanelet::Ids ids; + for (const auto & lanelet : LaneletWrapper::map()->laneletLayer) { + ids.push_back(lanelet.id()); + } + return ids; +} + +auto nearbyLaneletIds( + const Point & point, const double distance_thresh, const bool include_crosswalk, + const std::size_t search_count) -> lanelet::Ids +{ + auto isEmptyOrBeyondThreshold = [&distance_thresh](const auto & lanelets) { + return lanelets.empty() || lanelets.front().first > distance_thresh; + }; + + auto excludeSubtypeLanelets = + []( + const std::vector> & pair_distance_lanelet, + const char subtype[]) { + std::vector> filtered_lanelets; + for (const auto & pair : pair_distance_lanelet) { + if ( + pair.second.hasAttribute(lanelet::AttributeName::Subtype) && + pair.second.attribute(lanelet::AttributeName::Subtype).value() != subtype) { + filtered_lanelets.push_back(pair); + } + } + return filtered_lanelets; + }; + + auto nearest_lanelets = lanelet::geometry::findNearest( + LaneletWrapper::map()->laneletLayer, lanelet::BasicPoint2d(point.x, point.y), + static_cast(search_count)); + + /// @note check for current content, if not empty then optionally apply filter + if (isEmptyOrBeyondThreshold(nearest_lanelets)) { + return {}; + } else if (!include_crosswalk) { + nearest_lanelets = + excludeSubtypeLanelets(nearest_lanelets, lanelet::AttributeValueString::Crosswalk); + } + + /// @note check again + if (isEmptyOrBeyondThreshold(nearest_lanelets)) { + return {}; + } else { + lanelet::Ids target_lanelet_ids; + for (const auto & [distance, lanelet] : nearest_lanelets) { + if (distance <= distance_thresh) { + target_lanelet_ids.emplace_back(lanelet.id()); + } + } + return target_lanelet_ids; + } +} + +auto centerPoints(const lanelet::Ids & lanelet_ids) -> std::vector +{ + if (lanelet_ids.empty()) { + return {}; + } else { + std::vector center_points; + for (const auto & lanelet_id : lanelet_ids) { + auto points = centerPoints(lanelet_id); + center_points.insert(center_points.end(), points.begin(), points.end()); + } + center_points.erase( + std::unique(center_points.begin(), center_points.end()), center_points.end()); + return center_points; + } +} + +auto centerPoints(const lanelet::Id lanelet_id) -> std::vector +{ + return LaneletWrapper::centerPointsCache().getCenterPoints(lanelet_id, LaneletWrapper::map()); +} + +auto centerPointsSpline(const lanelet::Id lanelet_id) -> std::shared_ptr +{ + return LaneletWrapper::centerPointsCache().getCenterPointsSpline( + lanelet_id, LaneletWrapper::map()); +} + +auto nextLaneletIds(const lanelet::Id lanelet_id, const RoutingGraphType type) -> lanelet::Ids +{ + lanelet::Ids next_lanelet_ids; + const auto lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & following_lanelet : LaneletWrapper::routingGraph(type)->following(lanelet)) { + next_lanelet_ids.push_back(following_lanelet.id()); + } + return next_lanelet_ids; +} + +auto nextLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) -> lanelet::Ids +{ + std::set next_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto next_lanelet_ids = nextLaneletIds(lanelet_id, type); + next_lanelet_ids_set.insert(next_lanelet_ids.begin(), next_lanelet_ids.end()); + } + return lanelet::Ids(next_lanelet_ids_set.begin(), next_lanelet_ids_set.end()); +} + +auto nextLaneletIds( + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + lanelet::Ids next_lanelet_ids; + const auto & reference_lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & following_lanelet : + LaneletWrapper::routingGraph(type)->following(reference_lanelet)) { + if (following_lanelet.attributeOr("turn_direction", "else") == turn_direction) { + next_lanelet_ids.push_back(following_lanelet.id()); + } + } + return next_lanelet_ids; +} + +auto nextLaneletIds( + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + std::set next_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto next_lanelet_ids = nextLaneletIds(lanelet_id, turn_direction, type); + next_lanelet_ids_set.insert(next_lanelet_ids.begin(), next_lanelet_ids.end()); + } + return lanelet::Ids(next_lanelet_ids_set.begin(), next_lanelet_ids_set.end()); +} + +auto previousLaneletIds(const lanelet::Id lanelet_id, const RoutingGraphType type) -> lanelet::Ids +{ + lanelet::Ids previous_lanelet_ids; + const auto lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & previous_lanelet : LaneletWrapper::routingGraph(type)->previous(lanelet)) { + previous_lanelet_ids.push_back(previous_lanelet.id()); + } + return previous_lanelet_ids; +} + +auto previousLaneletIds(const lanelet::Ids & lanelet_ids, const RoutingGraphType type) + -> lanelet::Ids +{ + std::set previous_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto previous_lanelet_ids = previousLaneletIds(lanelet_id, type); + previous_lanelet_ids_set.insert(previous_lanelet_ids.begin(), previous_lanelet_ids.end()); + } + return lanelet::Ids(previous_lanelet_ids_set.begin(), previous_lanelet_ids_set.end()); +} + +auto previousLaneletIds( + const lanelet::Id lanelet_id, std::string_view turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + lanelet::Ids previous_lanelet_ids; + const auto reference_lanelet = LaneletWrapper::map()->laneletLayer.get(lanelet_id); + for (const auto & previous_lanelet : + LaneletWrapper::routingGraph(type)->previous(reference_lanelet)) { + if (previous_lanelet.attributeOr("turn_direction", "else") == turn_direction) { + previous_lanelet_ids.push_back(previous_lanelet.id()); + } + } + return previous_lanelet_ids; +} + +auto previousLaneletIds( + const lanelet::Ids & lanelet_ids, std::string_view turn_direction, const RoutingGraphType type) + -> lanelet::Ids +{ + std::set previous_lanelet_ids_set; + for (const auto & lanelet_id : lanelet_ids) { + auto previous_lanelet_ids = previousLaneletIds(lanelet_id, turn_direction, type); + previous_lanelet_ids_set.insert(previous_lanelet_ids.begin(), previous_lanelet_ids.end()); + } + return lanelet::Ids(previous_lanelet_ids_set.begin(), previous_lanelet_ids_set.end()); +} +} // namespace lanelet_map +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp new file mode 100644 index 00000000000..36c8e56d2bf --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -0,0 +1,118 @@ +// 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 + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +auto LaneletWrapper::activate(const std::string & lanelet_map_path) -> void +{ + lanelet_map_path_ = lanelet_map_path; + if (instance) { + std::lock_guard lock(mutex_); + instance.reset(); + } +} + +auto LaneletWrapper::map() -> lanelet::LaneletMapPtr { return getInstance().lanelet_map_ptr_; } + +auto LaneletWrapper::routingGraph(const RoutingGraphType type) + -> lanelet::routing::RoutingGraphConstPtr +{ + switch (type) { + case RoutingGraphType::VEHICLE: + return getInstance().vehicle_.graph; + case RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return getInstance().vehicle_with_road_shoulder_.graph; + case RoutingGraphType::PEDESTRIAN: + return getInstance().pedestrian_.graph; + default: + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); + } +} + +auto LaneletWrapper::trafficRules(const RoutingGraphType type) + -> lanelet::traffic_rules::TrafficRulesPtr +{ + switch (type) { + case RoutingGraphType::VEHICLE: + return getInstance().vehicle_.rules; + case RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return getInstance().vehicle_with_road_shoulder_.rules; + case RoutingGraphType::PEDESTRIAN: + return getInstance().pedestrian_.rules; + default: + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); + } +} + +auto LaneletWrapper::routeCache(const RoutingGraphType type) -> RouteCache & +{ + switch (type) { + case RoutingGraphType::VEHICLE: + return getInstance().vehicle_.route_cache; + case RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return getInstance().vehicle_with_road_shoulder_.route_cache; + case RoutingGraphType::PEDESTRIAN: + return getInstance().pedestrian_.route_cache; + default: + std::stringstream what; + what << "Invalid routing graph type: " << static_cast(type); + throw common::Error(what.str()); + } +} + +auto LaneletWrapper::centerPointsCache() -> CenterPointsCache & +{ + return getInstance().center_points_cache_; +} + +auto LaneletWrapper::laneletLengthCache() -> LaneletLengthCache & +{ + return getInstance().lanelet_length_cache_; +} + +LaneletWrapper::LaneletWrapper(const std::filesystem::path & lanelet_map_path) +: lanelet_map_ptr_(LaneletLoader::load(lanelet_map_path)), + vehicle_(lanelet_map_ptr_, lanelet::Locations::Germany, lanelet::Participants::Vehicle), + vehicle_with_road_shoulder_( + lanelet_map_ptr_, Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle), + pedestrian_(lanelet_map_ptr_, lanelet::Locations::Germany, lanelet::Participants::Pedestrian) +{ +} + +LaneletWrapper & LaneletWrapper::getInstance() +{ + std::lock_guard lock(mutex_); + if (!instance) { + if (!lanelet_map_path_.empty()) { + /// @note `new` is intentionally used here instead of `make_unique` since the LaneletWrapper constructor is private + instance.reset(new LaneletWrapper(lanelet_map_path_)); + } else { + THROW_SIMULATION_ERROR( + "LaneletWrapper::lanelet_map_path_ is empty! Please call lanelet_map::activate() first."); + } + } + return *instance; +} +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp new file mode 100644 index 00000000000..a0688cd5055 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -0,0 +1,491 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace pose +{ +auto toMapPose(const LaneletPose & lanelet_pose, const bool fill_pitch) -> PoseStamped +{ + using math::geometry::operator*; + using math::geometry::operator+=; + if ( + const auto canonicalized_lanelet_pose = + std::get>(pose::canonicalizeLaneletPose(lanelet_pose))) { + PoseStamped pose_stamped; + pose_stamped.header.frame_id = "map"; + const auto lanelet_spline = + lanelet_map::centerPointsSpline(canonicalized_lanelet_pose->lanelet_id); + /// @note map position + pose_stamped.pose = lanelet_spline->getPose(canonicalized_lanelet_pose->s); + pose_stamped.pose.position += + math::geometry::normalize(lanelet_spline->getNormalVector(canonicalized_lanelet_pose->s)) * + canonicalized_lanelet_pose->offset; + /// @note map orientation + const auto tangent_vector = lanelet_spline->getTangentVector(canonicalized_lanelet_pose->s); + const auto lanelet_rpy = + geometry_msgs::build() + .x(0.0) + .y( + fill_pitch ? std::atan2(-tangent_vector.z, std::hypot(tangent_vector.x, tangent_vector.y)) + : 0.0) + .z(std::atan2(tangent_vector.y, tangent_vector.x)); + pose_stamped.pose.orientation = + math::geometry::convertEulerAngleToQuaternion(lanelet_rpy) * + math::geometry::convertEulerAngleToQuaternion(canonicalized_lanelet_pose->rpy); + return pose_stamped; + } else { + THROW_SEMANTIC_ERROR( + "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, + ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, + ",rpy.z=", lanelet_pose.rpy.z, ") is invalid, please check lanelet length and connection."); + } +} + +auto isAltitudeMatching(const double current_altitude, const double target_altitude) -> bool +{ + /** + * @brief Justification for using a fixed `altitude_threshold` value of 1.0 [m]. + * + * Using a fixed `altitude_threshold` value of 1.0 [m] is justified because the + * entity's Z-position is always relative to its base. This eliminates the need + * to dynamically adjust the threshold based on the entity's dimensions, ensuring + * consistent altitude matching regardless of the entity type. + * + * The position of the entity is defined relative to its base, typically the center + * of the rear axle projected onto the ground in the case of vehicles. + * + * @note There is no technical basis for this value; it was determined based on experiments. + */ + static constexpr double altitude_threshold = 1.0; + return std::abs(current_altitude - target_altitude) <= altitude_threshold; +} + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance) + -> std::optional +{ + /// @note yaw_threshold_deg is used to determine whether the entity is going straight, + /// it defines the maximum allowed rotation with respect to the lanelet centerline. + constexpr double yaw_threshold_deg = 45.0; + + const auto lanelet_spline = lanelet_map::centerPointsSpline(lanelet_id); + if (const auto lanelet_pose_s = lanelet_spline->getSValue(map_pose, matching_distance); + !lanelet_pose_s) { + return std::nullopt; + } else if (const auto pose_on_centerline = lanelet_spline->getPose(lanelet_pose_s.value()); + !isAltitudeMatching(map_pose.position.z, pose_on_centerline.position.z)) { + return std::nullopt; + } else { + constexpr double yaw_range_min_rad = M_PI * yaw_threshold_deg / 180.0; + constexpr double yaw_range_max_rad = M_PI - yaw_range_min_rad; + if (const auto lanelet_pose_rpy = math::geometry::convertQuaternionToEulerAngle( + math::geometry::getRotation(pose_on_centerline.orientation, map_pose.orientation)); + std::fabs(lanelet_pose_rpy.z) > yaw_range_min_rad and + std::fabs(lanelet_pose_rpy.z) < yaw_range_max_rad) { + return std::nullopt; + } else { + double lanelet_pose_offset = std::sqrt( + lanelet_spline->getSquaredDistanceIn2D(map_pose.position, lanelet_pose_s.value())); + if (const double inner_product = math::geometry::innerProduct( + lanelet_spline->getNormalVector(lanelet_pose_s.value()), + lanelet_spline->getSquaredDistanceVector(map_pose.position, lanelet_pose_s.value())); + inner_product < 0) { + lanelet_pose_offset = lanelet_pose_offset * -1; + } + return traffic_simulator_msgs::build() + .lanelet_id(lanelet_id) + .s(lanelet_pose_s.value()) + .offset(lanelet_pose_offset) + .rpy(lanelet_pose_rpy); + } + } +} + +auto toLaneletPose( + const Pose & map_pose, const lanelet::Ids & lanelet_ids, const double matching_distance) + -> std::optional +{ + for (const auto & lanelet_id : lanelet_ids) { + if (const auto lanelet_pose = toLaneletPose(map_pose, lanelet_id, matching_distance); + lanelet_pose) { + return lanelet_pose.value(); + } + } + return std::nullopt; +} + +auto toLaneletPose( + const Pose & map_pose, const bool include_crosswalk, const double matching_distance) + -> std::optional +{ + /// @note Hardcoded parameter, this value has no technical basis and is determined based on experimentation. + /// @todo Add doxygen comments as soon as you know the meaning and rationale of the value. + constexpr double distance_threshold{0.1}; + constexpr std::size_t search_count{5}; + const auto nearby_lanelet_ids = lanelet_map::nearbyLaneletIds( + map_pose.position, distance_threshold, include_crosswalk, search_count); + return toLaneletPose(map_pose, nearby_lanelet_ids, matching_distance); +} + +auto toLaneletPose( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance, const RoutingGraphType type) -> std::optional +{ + const auto lanelet_id_using_bounding_box = matchToLane( + map_pose, bounding_box, include_crosswalk, matching_distance, + DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, type); + + if (!lanelet_id_using_bounding_box) { + return toLaneletPose(map_pose, include_crosswalk, matching_distance); + } else if ( + const auto lanelet_pose_using_bounding_box = + toLaneletPose(map_pose, lanelet_id_using_bounding_box.value(), matching_distance)) { + return lanelet_pose_using_bounding_box; + } + + for (const auto & previous_lanelet_id : + lanelet_map::previousLaneletIds(lanelet_id_using_bounding_box.value(), type)) { + if ( + const auto lanelet_pose_in_previous_lanelet = + toLaneletPose(map_pose, previous_lanelet_id, matching_distance)) { + return lanelet_pose_in_previous_lanelet; + } + } + + for (const auto & next_lanelet_id : + lanelet_map::nextLaneletIds(lanelet_id_using_bounding_box.value(), type)) { + if ( + const auto lanelet_pose_in_next_lanelet = + toLaneletPose(map_pose, next_lanelet_id, matching_distance)) { + return lanelet_pose_in_next_lanelet; + } + } + return toLaneletPose(map_pose, include_crosswalk, matching_distance); +} + +auto toLaneletPoses( + const Pose & map_pose, const lanelet::Id lanelet_id, const double matching_distance, + const bool include_opposite_direction, const RoutingGraphType type) -> std::vector +{ + std::vector lanelet_poses; + std::set new_lanelet_ids_set{lanelet_id}; + auto insertNewIds = [&](const auto & new_ids) { + new_lanelet_ids_set.insert(new_ids.begin(), new_ids.end()); + }; + insertNewIds(leftLaneletIds(lanelet_id, type, include_opposite_direction)); + insertNewIds(rightLaneletIds(lanelet_id, type, include_opposite_direction)); + insertNewIds(lanelet_map::previousLaneletIds({lanelet_id}, type)); + insertNewIds(lanelet_map::nextLaneletIds({lanelet_id}, type)); + for (const auto & new_lanelet_id : new_lanelet_ids_set) { + if (const auto & lanelet_pose = toLaneletPose(map_pose, new_lanelet_id, matching_distance)) { + lanelet_poses.emplace_back(lanelet_pose.value()); + } + } + return lanelet_poses; +} + +auto alternativeLaneletPoses(const LaneletPose & reference_lanelet_pose) -> std::vector +{ + const auto alternativesInPreviousLanelet = [](const auto & lanelet_pose) { + std::vector lanelet_poses_in_previous_lanelet; + for (const auto & previous_lanelet_id : + lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id)) { + const auto lanelet_pose_in_previous_lanelet = helper::constructLaneletPose( + previous_lanelet_id, lanelet_pose.s + lanelet_map::laneletLength(previous_lanelet_id), + lanelet_pose.offset); + if (const auto recursive_alternative_poses = + alternativeLaneletPoses(lanelet_pose_in_previous_lanelet); + recursive_alternative_poses.empty()) { + lanelet_poses_in_previous_lanelet.emplace_back(lanelet_pose_in_previous_lanelet); + } else { + lanelet_poses_in_previous_lanelet.insert( + lanelet_poses_in_previous_lanelet.end(), recursive_alternative_poses.begin(), + recursive_alternative_poses.end()); + } + } + return lanelet_poses_in_previous_lanelet; + }; + + const auto alternativesInNextLanelet = [](const auto & lanelet_pose) { + std::vector lanelet_poses_in_next_lanelet; + for (const auto & next_lanelet_id : lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id)) { + const auto lanelet_pose_in_next_lanelet = helper::constructLaneletPose( + next_lanelet_id, lanelet_pose.s - lanelet_map::laneletLength(lanelet_pose.lanelet_id), + lanelet_pose.offset); + if (const auto recursive_alternative_poses = + alternativeLaneletPoses(lanelet_pose_in_next_lanelet); + recursive_alternative_poses.empty()) { + lanelet_poses_in_next_lanelet.emplace_back(lanelet_pose_in_next_lanelet); + } else { + lanelet_poses_in_next_lanelet.insert( + lanelet_poses_in_next_lanelet.end(), recursive_alternative_poses.begin(), + recursive_alternative_poses.end()); + } + } + return lanelet_poses_in_next_lanelet; + }; + + /// @note If s value under 0, it means this pose is on the previous lanelet. + if (reference_lanelet_pose.s < 0) { + return alternativesInPreviousLanelet(reference_lanelet_pose); + } + /// @note If s value overs it's lanelet length, it means this pose is on the next lanelet. + else if ( + reference_lanelet_pose.s > (lanelet_map::laneletLength(reference_lanelet_pose.lanelet_id))) { + return alternativesInNextLanelet(reference_lanelet_pose); + } + /// @note If s value is in range [0,length_of_the_lanelet], return lanelet_pose. + else { + return {reference_lanelet_pose}; + } +} + +auto alongLaneletPose( + const LaneletPose & from_pose, const double distance, const RoutingGraphType type) -> LaneletPose +{ + auto lanelet_pose = from_pose; + lanelet_pose.s += distance; + if (lanelet_pose.s >= 0) { + while (lanelet_pose.s >= lanelet_map::laneletLength(lanelet_pose.lanelet_id)) { + auto next_lanelet_ids = + lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id, "straight", type); + if (next_lanelet_ids.empty()) { + /// @note if empty try to use other than "straight", but the first found + next_lanelet_ids = lanelet_map::nextLaneletIds(lanelet_pose.lanelet_id, type); + } + if (next_lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR( + "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", + from_pose.s + distance, "), next lanelet of id = ", lanelet_pose.lanelet_id, "is empty."); + } + lanelet_pose.s = lanelet_pose.s - lanelet_map::laneletLength(lanelet_pose.lanelet_id); + lanelet_pose.lanelet_id = next_lanelet_ids[0]; + } + } else { + while (lanelet_pose.s < 0) { + auto previous_lanelet_ids = + lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id, "straight", type); + if (previous_lanelet_ids.empty()) { + /// @note if empty try to use other than "straight", but the first found + previous_lanelet_ids = lanelet_map::previousLaneletIds(lanelet_pose.lanelet_id, type); + } + if (previous_lanelet_ids.empty()) { + THROW_SEMANTIC_ERROR( + "failed to calculate along pose (id,s) = (", from_pose.lanelet_id, ",", + from_pose.s + distance, "), next lanelet of id = ", lanelet_pose.lanelet_id, "is empty."); + } + lanelet_pose.s = lanelet_pose.s + lanelet_map::laneletLength(previous_lanelet_ids[0]); + lanelet_pose.lanelet_id = previous_lanelet_ids[0]; + } + } + return lanelet_pose; +} + +auto alongLaneletPose( + const LaneletPose & from_pose, const lanelet::Ids & route_lanelets, const double distance) + -> LaneletPose +{ + auto lanelet_pose = from_pose; + lanelet_pose.s += distance; + const auto canonicalized = canonicalizeLaneletPose(lanelet_pose, route_lanelets); + if ( + const auto & canonicalized_lanelet_pose = std::get>(canonicalized)) { + /// @note If canonicalize succeed, just return canonicalized pose + return canonicalized_lanelet_pose.value(); + } else { + /// @note If canonicalize failed, return lanelet pose as end of road + if (const auto end_of_road_lanelet_id = std::get>(canonicalized)) { + return traffic_simulator_msgs::build() + .lanelet_id(end_of_road_lanelet_id.value()) + .s(lanelet_pose.s <= 0 ? 0 : lanelet_map::laneletLength(end_of_road_lanelet_id.value())) + .offset(lanelet_pose.offset) + .rpy(lanelet_pose.rpy); + } else { + THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id."); + } + } +} + +/** + * @brief Canonicalizes a given LaneletPose by adjusting the longitudinal position (s) to ensure it + * lies within the bounds of the specified lanelet. If the position is out of bounds, it + * traverses to previous or next lanelets to find the canonicalized position. + * + * If the provided pose has a longitudinal position (s) less than 0, the function traverses + * to previous lanelets until a valid position is found or no previous lanelets exist. + * + * If the longitudinal position (s) exceeds the length of the current lanelet, the function traverses + * to next lanelets until the position is valid or no next lanelets exist. + * + * @param lanelet_pose The input LaneletPose to canonicalize, containing a lanelet ID and longitudinal position (s). + * @return A tuple where: + * - The first element is an optional canonicalized LaneletPose (std::nullopt if canonicalization fails). + * - The second element is an optional lanelet ID where the process stopped (std::nullopt if successful). + */ +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose) + -> std::tuple, std::optional> +{ + auto canonicalized_lanelet_pose = lanelet_pose; + while (canonicalized_lanelet_pose.s < 0) { + if (const auto previous_lanelet_ids = + lanelet_map::previousLaneletIds(canonicalized_lanelet_pose.lanelet_id); + previous_lanelet_ids.empty()) { + return {std::nullopt, canonicalized_lanelet_pose.lanelet_id}; + } else { + canonicalized_lanelet_pose.s += lanelet_map::laneletLength(previous_lanelet_ids[0]); + canonicalized_lanelet_pose.lanelet_id = previous_lanelet_ids[0]; + } + } + while (canonicalized_lanelet_pose.s > + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id)) { + if (const auto next_lanelet_ids = + lanelet_map::nextLaneletIds(canonicalized_lanelet_pose.lanelet_id); + next_lanelet_ids.empty()) { + return {std::nullopt, canonicalized_lanelet_pose.lanelet_id}; + } else { + canonicalized_lanelet_pose.s -= + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id); + canonicalized_lanelet_pose.lanelet_id = next_lanelet_ids[0]; + } + } + return {canonicalized_lanelet_pose, std::nullopt}; +} + +auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> std::tuple, std::optional> +{ + if (lanelet_pose.s < 0) { + /// @note When canonicalizing to backward lanelet_id, do not consider route + return canonicalizeLaneletPose(lanelet_pose); + } + auto canonicalized_lanelet_pose = lanelet_pose; + while (canonicalized_lanelet_pose.s > + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id)) { + /// @note When canonicalizing to forward lanelet_id, consider route + bool found_next_lanelet_in_route = false; + for (const auto & next_lanelet_id : + lanelet_map::nextLaneletIds(canonicalized_lanelet_pose.lanelet_id)) { + if ( + std::find(route_lanelets.begin(), route_lanelets.end(), next_lanelet_id) != + route_lanelets.end()) { + found_next_lanelet_in_route = true; + canonicalized_lanelet_pose.s -= + lanelet_map::laneletLength(canonicalized_lanelet_pose.lanelet_id); + canonicalized_lanelet_pose.lanelet_id = next_lanelet_id; + } + } + if (!found_next_lanelet_in_route) { + return {std::nullopt, canonicalized_lanelet_pose.lanelet_id}; + } + } + return {canonicalized_lanelet_pose, std::nullopt}; +} + +auto matchToLane( + const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk, + const double matching_distance, const double reduction_ratio, const RoutingGraphType type) + -> std::optional +{ + const auto absoluteHullPolygon = [&reduction_ratio, &bounding_box](const auto & pose) { + auto relative_hull = lanelet::matching::Hull2d{ + lanelet::BasicPoint2d{ + bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio, + bounding_box.center.y + bounding_box.dimensions.y * 0.5 * reduction_ratio}, + lanelet::BasicPoint2d{ + bounding_box.center.x - bounding_box.dimensions.x * 0.5 * reduction_ratio, + bounding_box.center.y - bounding_box.dimensions.y * 0.5 * reduction_ratio}}; + lanelet::BasicPolygon2d absolute_hull_polygon; + absolute_hull_polygon.reserve(relative_hull.size()); + for (const auto & relative_hull_point : relative_hull) { + absolute_hull_polygon.push_back(pose * relative_hull_point); + } + return absolute_hull_polygon; + }; + /// @note prepare object for matching + const auto yaw = math::geometry::convertQuaternionToEulerAngle(map_pose.orientation).z; + lanelet::matching::Object2d bounding_box_object; + bounding_box_object.pose.translation() = + lanelet::BasicPoint2d(map_pose.position.x, map_pose.position.y); + bounding_box_object.pose.linear() = Eigen::Rotation2D(yaw).matrix(); + bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box_object.pose); + /// @note find matches and optionally filter + auto matches = lanelet::matching::getDeterministicMatches( + *LaneletWrapper::map(), bounding_box_object, matching_distance); + if (!include_crosswalk) { + matches = + lanelet::matching::removeNonRuleCompliantMatches(matches, LaneletWrapper::trafficRules(type)); + } + /// @note find best match (minimize offset) + if (matches.empty()) { + return std::nullopt; + } else { + std::optional> min_pair_id_offset; + for (const auto & match : matches) { + if (const auto lanelet_pose = + pose::toLaneletPose(map_pose, match.lanelet.id(), matching_distance); + lanelet_pose && + (!min_pair_id_offset || lanelet_pose->offset < min_pair_id_offset->second)) { + min_pair_id_offset = std::make_pair(lanelet_pose->lanelet_id, lanelet_pose->offset); + } + } + return min_pair_id_offset ? std::optional(min_pair_id_offset->first) : std::nullopt; + } +} + +auto leftLaneletIds( + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids +{ + if (include_opposite_direction) { + throw common::Error( + "lanelet_wrapper::pose::leftLaneletIds with include_opposite_direction=true is not " + "implemented yet."); + } else { + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->lefts( + LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + } +} + +auto rightLaneletIds( + const lanelet::Id lanelet_id, const RoutingGraphType type, const bool include_opposite_direction) + -> lanelet::Ids +{ + if (include_opposite_direction) { + throw common::Error( + "lanelet_wrapper::pose::rightLaneletIds with include_opposite_direction=true is not " + "implemented " + "yet."); + } else { + return lanelet_map::laneletIds(LaneletWrapper::routingGraph(type)->rights( + LaneletWrapper::map()->laneletLayer.get(lanelet_id))); + } +} +} // namespace pose +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp similarity index 68% rename from simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp rename to simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp index 08b3c948461..07fade57250 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/traffic_rules.cpp @@ -14,16 +14,21 @@ #include -#include +#include -lanelet::traffic_rules::RegisterTrafficRules +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +const lanelet::traffic_rules::RegisterTrafficRules germanRoadShoulderPassableVehicleRules( - hdmap_utils::Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); + Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); -lanelet::traffic_rules::LaneChangeType -hdmap_utils::GermanRoadShoulderPassableVehicle::laneChangeType( +lanelet::traffic_rules::LaneChangeType GermanRoadShoulderPassableVehicle::laneChangeType( const lanelet::ConstLineString3d &, bool) const { /// @note allow lane-changes everywhere even if prohibited by lanelet2 map, because lane-change settings are not for entities but only for Autoware. return lanelet::traffic_rules::LaneChangeType::Both; } +} // namespace lanelet_wrapper +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp index 4d79b8c3755..5260db7c2e9 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_controller.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_controller.cpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -50,18 +51,12 @@ TrafficController::TrafficController( auto TrafficController::appendAutoSinks(const std::set & auto_sink_entity_types) -> void { - static constexpr double sink_radius = 1.0; - const auto hdmap_utils_ptr = entity_manager_ptr_->getHdmapUtils(); - for (const auto & lanelet_id : hdmap_utils_ptr->getLaneletIds()) { - if (hdmap_utils_ptr->getNextLaneletIds(lanelet_id).empty()) { - LaneletPose lanelet_pose; - lanelet_pose.lanelet_id = lanelet_id; - lanelet_pose.s = pose::laneletLength(lanelet_id, hdmap_utils_ptr); - const auto pose = pose::toMapPose(lanelet_pose, hdmap_utils_ptr); - const auto traffic_sink_config = TrafficSinkConfig( - sink_radius, pose.position, auto_sink_entity_types, std::make_optional(lanelet_id)); - addModule(despawn_, entity_manager_ptr_, traffic_sink_config); - } + /// @note Hard coded parameter, this value is radius of the traffic sink circle. + constexpr double sink_radius{1.0}; + for (const auto & [lanelet_id, pose] : lanelet_map::noNextLaneletPoses()) { + const auto traffic_sink_config = TrafficSinkConfig( + sink_radius, pose.position, auto_sink_entity_types, std::make_optional(lanelet_id)); + addModule(despawn_, entity_manager_ptr_, traffic_sink_config); } } diff --git a/simulation/traffic_simulator/src/traffic/traffic_source.cpp b/simulation/traffic_simulator/src/traffic/traffic_source.cpp index 68f82fdce61..347ea1cf7a6 100644 --- a/simulation/traffic_simulator/src/traffic/traffic_source.cpp +++ b/simulation/traffic_simulator/src/traffic/traffic_source.cpp @@ -17,6 +17,8 @@ #include #include #include +#include +#include #include namespace traffic_simulator @@ -90,8 +92,8 @@ auto TrafficSource::makeRandomPose( !nearby_lanelets.empty()) { // Get the altitude of the first nearby lanelet if ( - const auto altitude = - hdmap_utils_->getLaneletAltitude(nearby_lanelets.front(), random_pose, radius)) { + const auto altitude = traffic_simulator::lanelet_map::laneletAltitude( + nearby_lanelets.front(), random_pose, radius)) { random_pose.position.z = altitude.value(); } } @@ -185,18 +187,14 @@ auto TrafficSource::isPoseValid( return {true, std::nullopt}; } - if (const auto lanelet_pose = - hdmap_utils_->toLaneletPose(pose, std::holds_alternative(parameter)); - lanelet_pose) { + if ( + auto canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose( + pose, std::holds_alternative(parameter))) { /// @note reset orientation - to align the entity with lane - auto corrected_pose = lanelet_pose.value(); - corrected_pose.rpy.z = 0.0; - - auto out_pose = std::make_optional(corrected_pose, hdmap_utils_); - + canonicalized_lanelet_pose->alignOrientationToLanelet(); /// @note Step 3: check whether the bounding box can be outside lanelet if (not configuration_.require_footprint_fitting) { - return std::make_pair(true, out_pose); + return std::make_pair(true, canonicalized_lanelet_pose.value()); } /// @note Step 4: check whether the bounding box fits inside the lanelet @@ -205,17 +203,17 @@ auto TrafficSource::isPoseValid( not configuration_.require_footprint_fitting or validate_considering_crosswalk( math::geometry::transformPoints( - hdmap_utils_->toMapPose(corrected_pose).pose, bbox_corners), - corrected_pose.lanelet_id), - out_pose); + pose::toMapPose(canonicalized_lanelet_pose.value()), bbox_corners), + canonicalized_lanelet_pose->getLaneletId()), + canonicalized_lanelet_pose.value()); } else { return std::make_pair( not configuration_.require_footprint_fitting or validate( math::geometry::transformPoints( - hdmap_utils_->toMapPose(corrected_pose).pose, bbox_corners), - corrected_pose.lanelet_id), - out_pose); + pose::toMapPose(canonicalized_lanelet_pose.value()), bbox_corners), + canonicalized_lanelet_pose->getLaneletId()), + canonicalized_lanelet_pose.value()); } } else { return {false, std::nullopt}; diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 98bac102b88..cb2f1169d70 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -97,12 +98,12 @@ auto longitudinalDistance( */ constexpr double matching_distance = 5.0; - auto from_poses = hdmap_utils_ptr->toLaneletPoses( + auto from_poses = lanelet_wrapper::pose::toLaneletPoses( static_cast(from), static_cast(from).lanelet_id, matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); from_poses.emplace_back(from); - auto to_poses = hdmap_utils_ptr->toLaneletPoses( + auto to_poses = lanelet_wrapper::pose::toLaneletPoses( static_cast(to), static_cast(to).lanelet_id, matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); to_poses.emplace_back(to); @@ -112,9 +113,8 @@ auto longitudinalDistance( for (const auto & to_pose : to_poses) { if ( const auto distance = longitudinalDistance( - CanonicalizedLaneletPose(from_pose, hdmap_utils_ptr), - CanonicalizedLaneletPose(to_pose, hdmap_utils_ptr), false, include_opposite_direction, - routing_configuration, hdmap_utils_ptr)) { + CanonicalizedLaneletPose(from_pose), CanonicalizedLaneletPose(to_pose), false, + include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { distances.emplace_back(distance.value()); } } diff --git a/simulation/traffic_simulator/src/utils/lanelet_map.cpp b/simulation/traffic_simulator/src/utils/lanelet_map.cpp new file mode 100644 index 00000000000..5c7e5133932 --- /dev/null +++ b/simulation/traffic_simulator/src/utils/lanelet_map.cpp @@ -0,0 +1,40 @@ +// 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 + +namespace traffic_simulator +{ +inline namespace lanelet_map +{ +auto laneletLength(const lanelet::Id lanelet_id) -> double +{ + return lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); +} + +auto noNextLaneletPoses() -> std::vector> +{ + std::vector> no_next_lanelet_poses; + for (const auto & lanelet_id : lanelet_wrapper::lanelet_map::laneletIds()) { + if (lanelet_wrapper::lanelet_map::nextLaneletIds(lanelet_id).empty()) { + LaneletPose lanelet_pose; + lanelet_pose.lanelet_id = lanelet_id; + lanelet_pose.s = lanelet_map::laneletLength(lanelet_id); + no_next_lanelet_poses.emplace_back(lanelet_id, pose::toMapPose(lanelet_pose)); + } + } + return no_next_lanelet_poses; +} +} // namespace lanelet_map +} // namespace traffic_simulator diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index e8ea7673586..4be4a539d97 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -18,7 +18,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -49,19 +52,34 @@ auto quietNaNLaneletPose() -> LaneletPose .z(std::numeric_limits::quiet_NaN())); } -auto canonicalize( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) - -> std::optional +/// @note Conversions +auto canonicalize(const LaneletPose & lanelet_pose) -> LaneletPose { - if (lanelet_pose == LaneletPose()) { - return std::nullopt; + if ( + const auto canonicalized = std::get>( + lanelet_wrapper::pose::canonicalizeLaneletPose(lanelet_pose))) { + return canonicalized.value(); } else { - try { - return CanonicalizedLaneletPose(lanelet_pose, hdmap_utils_ptr); - } catch (const common::SemanticError &) { - return std::nullopt; - } + THROW_SEMANTIC_ERROR( + "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, + ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, + ",rpy.z=", lanelet_pose.rpy.z, ") is invalid, please check lanelet length and connection."); + } +} + +auto canonicalize(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets) + -> LaneletPose +{ + if ( + const auto canonicalized = std::get>( + lanelet_wrapper::pose::canonicalizeLaneletPose(lanelet_pose, route_lanelets))) { + return canonicalized.value(); + } else { + THROW_SEMANTIC_ERROR( + "Lanelet pose (id=", lanelet_pose.lanelet_id, ",s=", lanelet_pose.s, + ",offset=", lanelet_pose.offset, ",rpy.x=", lanelet_pose.rpy.x, ",rpy.y=", lanelet_pose.rpy.y, + ",rpy.z=", lanelet_pose.rpy.z, + ") is invalid, please check lanelet length, connection and entity route."); } } @@ -70,23 +88,42 @@ auto toMapPose(const CanonicalizedLaneletPose & lanelet_pose) -> geometry_msgs:: return static_cast(lanelet_pose); } -auto toMapPose( - const LaneletPose & lanelet_pose, - const std::shared_ptr & hdmap_utils_ptr) -> geometry_msgs::msg::Pose +auto toMapPose(const LaneletPose & lanelet_pose) -> geometry_msgs::msg::Pose { - return hdmap_utils_ptr - ->toMapPose(lanelet_pose, CanonicalizedLaneletPose::getConsiderPoseByRoadSlope()) + return lanelet_wrapper::pose::toMapPose( + lanelet_pose, CanonicalizedLaneletPose::getConsiderPoseByRoadSlope()) .pose; } +auto alternativeLaneletPoses(const LaneletPose & lanelet_pose) -> std::vector +{ + return lanelet_wrapper::pose::alternativeLaneletPoses(lanelet_pose); +} + +auto toCanonicalizedLaneletPose(const LaneletPose & lanelet_pose) + -> std::optional +{ + if (lanelet_pose == LaneletPose()) { + return std::nullopt; + } else { + try { + return CanonicalizedLaneletPose(lanelet_pose); + } catch (const common::SemanticError &) { + return std::nullopt; + } + } +} + auto toCanonicalizedLaneletPose( - const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk, - const std::shared_ptr & hdmap_utils_ptr) + const geometry_msgs::msg::Pose & map_pose, const bool include_crosswalk) -> std::optional { /// @todo here matching_distance should be passed - if (const auto pose = hdmap_utils_ptr->toLaneletPose(map_pose, include_crosswalk)) { - return canonicalize(pose.value(), hdmap_utils_ptr); + constexpr double matching_distance{1.0}; + if ( + const auto pose = + lanelet_wrapper::pose::toLaneletPose(map_pose, include_crosswalk, matching_distance)) { + return toCanonicalizedLaneletPose(pose.value()); } else { return std::nullopt; } @@ -95,13 +132,12 @@ auto toCanonicalizedLaneletPose( auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const double matching_distance) -> std::optional { if ( - const auto pose = hdmap_utils_ptr->toLaneletPose( + const auto pose = lanelet_wrapper::pose::toLaneletPose( map_pose, bounding_box, include_crosswalk, matching_distance)) { - return canonicalize(pose.value(), hdmap_utils_ptr); + return toCanonicalizedLaneletPose(pose.value()); } else { return std::nullopt; } @@ -111,20 +147,19 @@ auto toCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const double matching_distance) -> std::optional { std::optional lanelet_pose; if (!unique_route_lanelets.empty()) { lanelet_pose = - hdmap_utils_ptr->toLaneletPose(map_pose, unique_route_lanelets, matching_distance); + lanelet_wrapper::pose::toLaneletPose(map_pose, unique_route_lanelets, matching_distance); } if (!lanelet_pose) { - lanelet_pose = - hdmap_utils_ptr->toLaneletPose(map_pose, bounding_box, include_crosswalk, matching_distance); + lanelet_pose = lanelet_wrapper::pose::toLaneletPose( + map_pose, bounding_box, include_crosswalk, matching_distance); } if (lanelet_pose) { - return canonicalize(lanelet_pose.value(), hdmap_utils_ptr); + return toCanonicalizedLaneletPose(lanelet_pose.value()); } else { return std::nullopt; } @@ -192,6 +227,15 @@ auto updatePositionForLaneletTransition( next_lanelet_longitudinal_d); } } + +/// @note Relative msg::Pose +auto isAltitudeMatching( + const CanonicalizedLaneletPose & lanelet_pose, + const CanonicalizedLaneletPose & target_lanelet_pose) -> bool +{ + return lanelet_wrapper::pose::isAltitudeMatching( + lanelet_pose.getAltitude(), target_lanelet_pose.getAltitude()); +} auto relativePose(const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to) -> std::optional @@ -235,9 +279,11 @@ auto boundingBoxRelativePose( return std::nullopt; } +/// @note Relative LaneletPose +/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const traffic_simulator::RoutingConfiguration & routing_configuration, + const RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; @@ -260,12 +306,13 @@ auto relativeLaneletPose( return position; } +/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto boundingBoxRelativeLaneletPose( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, - const traffic_simulator::RoutingConfiguration & routing_configuration, + const RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; @@ -288,6 +335,7 @@ auto boundingBoxRelativeLaneletPose( return position; } +/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto isInLanelet( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const lanelet::Id lanelet_id, const double tolerance, const std::shared_ptr & hdmap_utils_ptr) -> bool @@ -299,8 +347,7 @@ auto isInLanelet( if (isSameLaneletId(canonicalized_lanelet_pose, lanelet_id)) { return true; } else { - const auto start_lanelet_pose = - helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0, hdmap_utils_ptr); + const auto start_lanelet_pose = helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0); if (const auto distance_to_start_lanelet_pose = longitudinalDistance( start_lanelet_pose, canonicalized_lanelet_pose, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr); @@ -310,7 +357,7 @@ auto isInLanelet( } const auto end_lanelet_pose = helper::constructCanonicalizedLaneletPose( - lanelet_id, hdmap_utils_ptr->getLaneletLength(lanelet_id), 0.0, hdmap_utils_ptr); + lanelet_id, lanelet_wrapper::lanelet_map::laneletLength(lanelet_id), 0.0); if (const auto distance_to_end_lanelet_pose = longitudinalDistance( canonicalized_lanelet_pose, end_lanelet_pose, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr); @@ -322,20 +369,19 @@ auto isInLanelet( return false; } +auto isInLanelet(const geometry_msgs::msg::Point & point, const lanelet::Id lanelet_id) -> bool +{ + return lanelet_wrapper::lanelet_map::isInLanelet(lanelet_id, point); +} + +/// @todo HdMapUtils will be removed when lanelet_wrapper::distance is added auto isAtEndOfLanelets( const CanonicalizedLaneletPose & canonicalized_lanelet_pose, const std::shared_ptr & hdmap_utils_ptr) -> bool { const auto lanelet_pose = static_cast(canonicalized_lanelet_pose); return hdmap_utils_ptr->getFollowingLanelets(lanelet_pose.lanelet_id).size() == 1 && - hdmap_utils_ptr->getLaneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; -} - -auto laneletLength( - const lanelet::Id lanelet_id, const std::shared_ptr & hdmap_utils_ptr) - -> double -{ - return hdmap_utils_ptr->getLaneletLength(lanelet_id); + lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s; } namespace pedestrian @@ -349,46 +395,44 @@ auto transformToCanonicalizedLaneletPose( const geometry_msgs::msg::Pose & map_pose, const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const lanelet::Ids & unique_route_lanelets, const bool include_crosswalk, - const double matching_distance, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const double matching_distance) -> std::optional { if ( const auto canonicalized_lanelet_pose = toCanonicalizedLaneletPose( - map_pose, bounding_box, unique_route_lanelets, include_crosswalk, matching_distance, - hdmap_utils_ptr)) { + map_pose, bounding_box, unique_route_lanelets, include_crosswalk, matching_distance)) { return canonicalized_lanelet_pose; } /** * @note Hard coded parameter. 2.0 is a matching threshold for lanelet. * In this branch, the algorithm only consider entity pose. */ - if (const auto lanelet_pose = hdmap_utils_ptr->toLaneletPose(map_pose, include_crosswalk, 2.0)) { - const auto canonicalized_tuple = hdmap_utils_ptr->canonicalizeLaneletPose(lanelet_pose.value()); + if ( + const auto lanelet_pose = + lanelet_wrapper::pose::toLaneletPose(map_pose, include_crosswalk, 2.0)) { + const auto canonicalized_tuple = + lanelet_wrapper::pose::canonicalizeLaneletPose(lanelet_pose.value()); if ( const auto canonicalized_lanelet_pose = std::get>(canonicalized_tuple)) { - return canonicalize(lanelet_pose.value(), hdmap_utils_ptr); + return toCanonicalizedLaneletPose(lanelet_pose.value()); } else { /// @note If canonicalize failed, set end of road lanelet pose. if ( const auto end_of_road_lanelet_id = std::get>(canonicalized_tuple)) { if (lanelet_pose.value().s < 0) { - return CanonicalizedLaneletPose( - traffic_simulator_msgs::build() - .lanelet_id(end_of_road_lanelet_id.value()) - .s(0.0) - .offset(lanelet_pose.value().offset) - .rpy(lanelet_pose.value().rpy), - hdmap_utils_ptr); + return CanonicalizedLaneletPose(traffic_simulator_msgs::build() + .lanelet_id(end_of_road_lanelet_id.value()) + .s(0.0) + .offset(lanelet_pose.value().offset) + .rpy(lanelet_pose.value().rpy)); } else { return CanonicalizedLaneletPose( traffic_simulator_msgs::build() .lanelet_id(end_of_road_lanelet_id.value()) - .s(hdmap_utils_ptr->getLaneletLength(end_of_road_lanelet_id.value())) + .s(lanelet_wrapper::lanelet_map::laneletLength(end_of_road_lanelet_id.value())) .offset(lanelet_pose.value().offset) - .rpy(lanelet_pose.value().rpy), - hdmap_utils_ptr); + .rpy(lanelet_pose.value().rpy)); } } else { THROW_SIMULATION_ERROR("Failed to find trailing lanelet_id for LaneletPose estimation."); diff --git a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp index f4f949deac1..3158d8124e8 100644 --- a/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp +++ b/simulation/traffic_simulator/test/src/behavior/test_route_planner.cpp @@ -30,12 +30,12 @@ class RoutePlannerTest : public testing::Test { protected: RoutePlannerTest() - : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), - planner(traffic_simulator::RoutingConfiguration().routing_graph_type, hdmap_utils_ptr) + : planner( + traffic_simulator::RoutingConfiguration().routing_graph_type, makeHdMapUtilsSharedPointer()) { + activateLaneletWrapper("standard_map"); } - std::shared_ptr hdmap_utils_ptr; traffic_simulator::RoutePlanner planner; }; @@ -46,9 +46,8 @@ class RoutePlannerTest : public testing::Test TEST_F(RoutePlannerTest, getGoalPoses) { const auto in_poses = std::vector{ - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468)}; + makeCanonicalizedLaneletPose(120659), makeCanonicalizedLaneletPose(120660), + makeCanonicalizedLaneletPose(34468)}; planner.setWaypoints(in_poses); @@ -70,9 +69,8 @@ TEST_F(RoutePlannerTest, getGoalPoses) TEST_F(RoutePlannerTest, getGoalPosesInWorldFrame) { const auto in_poses = std::vector{ - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660), - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468)}; + makeCanonicalizedLaneletPose(120659), makeCanonicalizedLaneletPose(120660), + makeCanonicalizedLaneletPose(34468)}; planner.setWaypoints(in_poses); @@ -92,9 +90,8 @@ TEST_F(RoutePlannerTest, getRouteLanelets_horizon) { const lanelet::Id id_target = 34579; - planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); - auto route = - planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 1000.0); + planner.setWaypoints({makeCanonicalizedLaneletPose(id_target)}); + auto route = planner.getRouteLanelets(makeCanonicalizedLaneletPose(120659), 1000.0); EXPECT_TRUE(std::find(route.begin(), route.end(), id_target) != route.end()); } @@ -107,9 +104,8 @@ TEST_F(RoutePlannerTest, getRouteLanelets_noHorizon) { lanelet::Id id_target = 34579; - planner.setWaypoints({makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_target)}); - const auto route = - planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 100.0); + planner.setWaypoints({makeCanonicalizedLaneletPose(id_target)}); + const auto route = planner.getRouteLanelets(makeCanonicalizedLaneletPose(120659), 100.0); EXPECT_FALSE(std::find(route.begin(), route.end(), id_target) != route.end()); } @@ -124,8 +120,7 @@ TEST_F(RoutePlannerTest, getRouteLanelets_empty) const lanelet::Ids following_ids({120659, 120660, 34468, 34465, 34462}); planner.setWaypoints({}); - const auto route = - planner.getRouteLanelets(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), 100.0); + const auto route = planner.getRouteLanelets(makeCanonicalizedLaneletPose(120659), 100.0); EXPECT_EQ(route.size(), following_ids.size()); for (size_t i = 0; i < route.size(); i++) { diff --git a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp index 24d74f063ce..eb03e9970d9 100644 --- a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp +++ b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp @@ -28,8 +28,10 @@ int main(int argc, char ** argv) class CanonicalizedLaneletPoseTest : public testing::Test { protected: - CanonicalizedLaneletPoseTest() : hdmap_utils(makeHdMapUtilsSharedPointer()) {} - + CanonicalizedLaneletPoseTest() : hdmap_utils(makeHdMapUtilsSharedPointer()) + { + activateLaneletWrapper("standard_map"); + } std::shared_ptr hdmap_utils; }; @@ -40,8 +42,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute_invalid) { EXPECT_THROW( CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), lanelet::Ids{}, - hdmap_utils), + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), lanelet::Ids{}), std::runtime_error); } @@ -53,8 +54,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute) std::shared_ptr pose; EXPECT_NO_THROW( pose = std::make_shared( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), lanelet::Ids{120659}, - hdmap_utils)); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), lanelet::Ids{120659})); EXPECT_LANELET_POSE_EQ( static_cast(*pose), traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); @@ -67,7 +67,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute_inval { EXPECT_THROW( CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), hdmap_utils), + traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0)), std::runtime_error); } @@ -79,7 +79,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute) std::shared_ptr pose; EXPECT_NO_THROW( pose = std::make_shared( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils)); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0))); EXPECT_LANELET_POSE_EQ( static_cast(*pose), traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); @@ -91,7 +91,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute) TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_copyConstructor) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_copy(pose); EXPECT_LANELET_POSE_EQ( static_cast(pose), @@ -104,7 +104,7 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_copyConstructor) TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_moveConstructor) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose2(pose); const CanonicalizedLaneletPose pose_move = std::move(pose2); EXPECT_LANELET_POSE_EQ( @@ -118,9 +118,9 @@ TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_moveConstructor) TEST_F(CanonicalizedLaneletPoseTest, copyAssignment) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); CanonicalizedLaneletPose pose_assign( - traffic_simulator::helper::constructLaneletPose(34468, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(34468, 0.0, 0.0)); pose_assign = pose; @@ -135,7 +135,7 @@ TEST_F(CanonicalizedLaneletPoseTest, copyAssignment) TEST_F(CanonicalizedLaneletPoseTest, conversionLaneletPose) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const traffic_simulator::LaneletPose pose_casted = static_cast(pose); @@ -151,7 +151,7 @@ TEST_F(CanonicalizedLaneletPoseTest, conversionLaneletPose) TEST_F(CanonicalizedLaneletPoseTest, conversionPose) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const geometry_msgs::msg::Pose pose1 = makePose(makePoint(3822.3815, 73784.9618, -1.761), makeQuaternionFromYaw(2.060578777273)); @@ -165,7 +165,7 @@ TEST_F(CanonicalizedLaneletPoseTest, conversionPose) TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_true) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, -10.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, -10.0, 0.0)); EXPECT_TRUE(pose.hasAlternativeLaneletPose()); } @@ -176,7 +176,7 @@ TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_true) TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_false) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 10.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 10.0, 0.0)); EXPECT_FALSE(pose.hasAlternativeLaneletPose()); } @@ -187,7 +187,7 @@ TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_false) TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_empty) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 20.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 20.0, 0.0)); const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); @@ -206,8 +206,7 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout */ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_single) { - CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(34666, -20.0, 0.0), hdmap_utils); + CanonicalizedLaneletPose pose(traffic_simulator::helper::constructLaneletPose(34666, -20.0, 0.0)); const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); @@ -227,7 +226,7 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_multiple) { CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, -20.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, -20.0, 0.0)); const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); @@ -265,13 +264,13 @@ TEST(CanonicalizedLaneletPose, setConsiderPoseByRoadSlope) TEST_F(CanonicalizedLaneletPoseTest, operatorLessEqual) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_TRUE(pose_less <= pose); EXPECT_TRUE(pose_equal <= pose); @@ -284,13 +283,13 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorLessEqual) TEST_F(CanonicalizedLaneletPoseTest, operatorLess) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_TRUE(pose_less < pose); EXPECT_FALSE(pose_equal < pose); @@ -303,13 +302,13 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorLess) TEST_F(CanonicalizedLaneletPoseTest, operatorGreaterEqual) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_FALSE(pose_less >= pose); EXPECT_TRUE(pose_equal >= pose); @@ -322,13 +321,13 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorGreaterEqual) TEST_F(CanonicalizedLaneletPoseTest, operatorGreater) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_equal( - traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0)); const CanonicalizedLaneletPose pose_less( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose_greater( - traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0)); EXPECT_FALSE(pose_less > pose); EXPECT_FALSE(pose_equal > pose); @@ -341,9 +340,9 @@ TEST_F(CanonicalizedLaneletPoseTest, operatorGreater) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_same) { const CanonicalizedLaneletPose pose1( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); const CanonicalizedLaneletPose pose2( - traffic_simulator::helper::constructLaneletPose(120659, 1.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 1.0, 0.0)); EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose1, pose2)); } @@ -354,9 +353,8 @@ TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_same) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_different) { const CanonicalizedLaneletPose pose1( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); - CanonicalizedLaneletPose pose2( - traffic_simulator::helper::constructLaneletPose(34606, 1.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); + CanonicalizedLaneletPose pose2(traffic_simulator::helper::constructLaneletPose(34606, 1.0, 0.0)); EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose1, pose2)); } @@ -367,7 +365,7 @@ TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_different) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_same) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose, 120659)); } @@ -378,7 +376,7 @@ TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_same) TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_different) { const CanonicalizedLaneletPose pose( - traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0)); EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose, 34606)); } diff --git a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp index 987b8220f3e..8ba79375d15 100644 --- a/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_misc_object_entity.cpp @@ -36,8 +36,8 @@ class MiscObjectEntityTest_HdMapUtils : public testing::Test MiscObjectEntityTest_HdMapUtils() : hdmap_utils_ptr(makeHdMapUtilsSharedPointer()), entity_name("misc_object_entity") { + activateLaneletWrapper("standard_map"); } - std::shared_ptr hdmap_utils_ptr; const std::string entity_name; }; @@ -47,9 +47,9 @@ class MiscObjectEntityTest_FullObject : public MiscObjectEntityTest_HdMapUtils protected: MiscObjectEntityTest_FullObject() : id(120659), - pose(makeCanonicalizedLaneletPose(hdmap_utils_ptr, id)), + pose(makeCanonicalizedLaneletPose(id)), bbox(makeBoundingBox()), - status(makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, 0.0, entity_name)), + status(makeCanonicalizedEntityStatus(pose, bbox, 0.0, entity_name)), misc_object( entity_name, status, hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}), entity_base(&misc_object) @@ -70,21 +70,22 @@ class MiscObjectEntityTest_FullObject : public MiscObjectEntityTest_HdMapUtils TEST_F(MiscObjectEntityTest_HdMapUtils, getCurrentAction_npcNotStarted) { auto non_canonicalized_status = makeEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), 0.0, - entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT); non_canonicalized_status.action_status.current_action = "current_action_name"; const auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, traffic_simulator::entity_status::CanonicalizedEntityStatus( - non_canonicalized_status, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659)), + non_canonicalized_status, makeCanonicalizedLaneletPose(120659)), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); EXPECT_EQ(blob.getCurrentAction(), "current_action_name"); } /** - * @note Test function behavior when absolute speed change is requested - the goal is to test throwing error. + * @note Test function behavior when absolute speed change is requested - the goal is to test + * throwing error. */ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absolute) { @@ -92,34 +93,34 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absolute) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestSpeedChange(10.0, false), common::SemanticError); } /** - * @note Test function behavior when relative speed change is requested - the goal is to test throwing error. + * @note Test function behavior when relative speed change is requested - the goal is to test + * throwing error. */ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_relative) { - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659); + auto pose = makeCanonicalizedLaneletPose(120659); auto bbox = makeBoundingBox(); auto blob = traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, pose, bbox, 0.0, entity_name, - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + pose, bbox, 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}); std::unordered_map others; others.emplace( - "other_entity", makeCanonicalizedEntityStatus( - hdmap_utils_ptr, pose, bbox, 17.0, "other_entity_name", - traffic_simulator_msgs::msg::EntityType::MISC_OBJECT)); + "other_entity", + makeCanonicalizedEntityStatus( + pose, bbox, 17.0, "other_entity_name", traffic_simulator_msgs::msg::EntityType::MISC_OBJECT)); blob.setOtherStatus(others); EXPECT_THROW( @@ -140,8 +141,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestSpeedChange_absoluteTransition) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestSpeedChange( 10.0, traffic_simulator::speed_change::Transition::AUTO, @@ -161,11 +162,11 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAssignRoute_laneletPose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAssignRoute(std::vector{ - makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)}), + makeCanonicalizedLaneletPose(120660)}), common::SemanticError); } @@ -179,8 +180,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAssignRoute_pose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAssignRoute( std::vector{makePose(makePoint(3759.34, 73791.38))}), @@ -197,10 +198,10 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAcquirePosition_laneletPose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) - .requestAcquirePosition(makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120660)), + .requestAcquirePosition(makeCanonicalizedLaneletPose(120660)), common::SemanticError); } @@ -214,8 +215,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, requestAcquirePosition_pose) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .requestAcquirePosition(makePose(makePoint(3759.34, 73791.38))), common::SemanticError); @@ -230,8 +231,8 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getRouteLanelets) traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 120659), makeBoundingBox(), - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makeCanonicalizedLaneletPose(120659), makeBoundingBox(), 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getRouteLanelets(100.0), common::SemanticError); @@ -350,8 +351,7 @@ TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetLaneletP auto other_status = std::unordered_map{}; other_status.emplace( - target_name, - makeCanonicalizedEntityStatus(hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), bbox)); + target_name, makeCanonicalizedEntityStatus(makePose(makePoint(3810.0, 73745.0)), bbox)); entity_base->setOtherStatus(other_status); @@ -375,9 +375,7 @@ TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetName) auto other_status = std::unordered_map{}; other_status.emplace( - target_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468, 5.0), bbox)); + target_name, makeCanonicalizedEntityStatus(makeCanonicalizedLaneletPose(34468, 5.0), bbox)); entity_base->setOtherStatus(other_status); EXPECT_THROW( @@ -402,9 +400,7 @@ TEST_F(MiscObjectEntityTest_FullObject, requestLaneChange_relativeTargetInvalid) auto other_status = std::unordered_map{}; other_status.emplace( - target_name, - makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makeCanonicalizedLaneletPose(hdmap_utils_ptr, 34468, 5.0), bbox)); + target_name, makeCanonicalizedEntityStatus(makeCanonicalizedLaneletPose(34468, 5.0), bbox)); entity_base->setOtherStatus(other_status); EXPECT_THROW( @@ -463,8 +459,8 @@ TEST_F( EXPECT_FALSE(traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 1.0, - 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), + makePose(makePoint(3810.0, 73745.0)), makeBoundingBox(), 1.0, 0.0, entity_name, + traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), hdmap_utils_ptr, traffic_simulator_msgs::msg::MiscObjectParameters{}) .getCanonicalizedLaneletPose(5.0) .has_value()); @@ -481,7 +477,6 @@ TEST_F(MiscObjectEntityTest_HdMapUtils, getCanonicalizedLaneletPose_onRoadAndCro traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3766.1, 73738.2), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), makeBoundingBox(), 1.0, 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), @@ -502,7 +497,6 @@ TEST_F( traffic_simulator::entity::MiscObjectEntity( entity_name, makeCanonicalizedEntityStatus( - hdmap_utils_ptr, makePose(makePoint(3764.5, 73737.5), makeQuaternionFromYaw((120.0) * M_PI / 180.0)), makeBoundingBox(), 1.0, 0.0, entity_name, traffic_simulator_msgs::msg::EntityType::MISC_OBJECT), diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp index e6ba6873f0d..f3bca97aa5a 100644 --- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp +++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include "../expect_eq_macros.hpp" #include "../helper_functions.hpp" @@ -42,6 +44,7 @@ class HdMapUtilsTest_StandardMap : public testing::Test .longitude(139.78066608243) .altitude(0.0)) { + activateLaneletWrapper("standard_map"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -58,6 +61,7 @@ class HdMapUtilsTest_WithRoadShoulderMap : public testing::Test .longitude(139.78066608243) .altitude(0.0)) { + activateLaneletWrapper("with_road_shoulder"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -74,6 +78,7 @@ class HdMapUtilsTest_EmptyMap : public testing::Test .longitude(0.0) .altitude(0.0)) { + activateLaneletWrapper("empty"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -90,6 +95,7 @@ class HdMapUtilsTest_FourTrackHighwayMap : public testing::Test .longitude(138.8024583466017) .altitude(0.0)) { + activateLaneletWrapper("four_track_highway"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -106,6 +112,7 @@ class HdMapUtilsTest_CrossroadsWithStoplinesMap : public testing::Test .longitude(139.9009591876285) .altitude(0.0)) { + activateLaneletWrapper("crossroads_with_stoplines"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -121,6 +128,9 @@ class HdMapUtilsTest_KashiwanohaMap : public testing::Test .longitude(0.0) .altitude(0.0)) { + const auto lanelet_path = + ament_index_cpp::get_package_share_directory("kashiwanoha_map") + "/map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); } hdmap_utils::HdMapUtils hdmap_utils; @@ -137,6 +147,7 @@ class HdMapUtilsTest_IntersectionMap : public testing::Test .longitude(139.74821144562) .altitude(0.0)) { + activateLaneletWrapper("intersection"); } hdmap_utils::HdMapUtils hdmap_utils; @@ -189,16 +200,20 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane) { const auto bbox = makeSmallBoundingBox(); { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(120659, 1)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(120659, 1)) + .pose, + bbox, false); EXPECT_TRUE(id); EXPECT_EQ(id.value(), 120659); } { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34411, 1)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34411, 1)) + .pose, + bbox, false); EXPECT_TRUE(id); EXPECT_EQ(id.value(), 34411); } @@ -213,16 +228,20 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane_includeCrosswalk) { auto bbox = makeSmallBoundingBox(); { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34399, 1)).pose, bbox, - true); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34399, 1)) + .pose, + bbox, true); EXPECT_TRUE(id.has_value()); EXPECT_EQ(id.value(), 34399); } { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34399, 1)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34399, 1)) + .pose, + bbox, false); if (id.has_value()) { EXPECT_NE(id.value(), 34399); } @@ -240,15 +259,19 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane_noMatch) { auto bbox = makeSmallBoundingBox(); { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34392, 0)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34392, 0)) + .pose, + bbox, false); EXPECT_FALSE(id.has_value()); } { - const auto id = hdmap_utils.matchToLane( - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34378, 0)).pose, bbox, - false); + const auto id = traffic_simulator::lanelet_wrapper::pose::matchToLane( + traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34378, 0)) + .pose, + bbox, false); EXPECT_FALSE(id.has_value()); } } @@ -261,11 +284,13 @@ TEST_F(HdMapUtilsTest_StandardMap, matchToLane_noMatch) TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_insideDistance) { EXPECT_DOUBLE_EQ( - hdmap_utils.getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) .s, 30.0); EXPECT_EQ( - hdmap_utils.getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), 30.0) .lanelet_id, 34513); } @@ -279,14 +304,14 @@ TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_insideDistance) TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_outsideDistance) { EXPECT_EQ( - hdmap_utils.getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), 30) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), 30) .lanelet_id, 34513); EXPECT_EQ( - hdmap_utils - .getAlongLaneletPose( - traffic_simulator::helper::constructLaneletPose(34513, 0), - hdmap_utils.getLaneletLength(34513) + 10.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34513) + 10.0) .lanelet_id, 34510); } @@ -300,15 +325,15 @@ TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_outsideDistance) TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_negativeDistance) { EXPECT_EQ( - hdmap_utils - .getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) .lanelet_id, 34684); EXPECT_DOUBLE_EQ( - hdmap_utils - .getAlongLaneletPose(traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( + traffic_simulator::helper::constructLaneletPose(34513, 0), -10.0) .s, - hdmap_utils.getLaneletLength(34684) - 10.0); + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34684) - 10.0); } /** @@ -319,7 +344,7 @@ TEST_F(HdMapUtilsTest_StandardMap, AlongLaneletPose_negativeDistance) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_afterLast) { EXPECT_THROW( - hdmap_utils.getAlongLaneletPose( + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( traffic_simulator::helper::constructLaneletPose(206, 15.0), 30.0), common::SemanticError); } @@ -332,7 +357,7 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_afterLast) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_beforeFirst) { EXPECT_THROW( - hdmap_utils.getAlongLaneletPose( + traffic_simulator::lanelet_wrapper::pose::alongLaneletPose( traffic_simulator::helper::constructLaneletPose(3002178, 15.0), -30.0), common::SemanticError); } @@ -347,15 +372,16 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, AlongLaneletPose_beforeFirst) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeNegative) { double non_canonicalized_lanelet_s = -22.0; - const auto canonicalized_lanelet_pose = - std::get>(hdmap_utils.canonicalizeLaneletPose( + const auto canonicalized_lanelet_pose = std::get>( + traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose( traffic_simulator::helper::constructLaneletPose(34564, non_canonicalized_lanelet_s))); EXPECT_EQ(canonicalized_lanelet_pose.value().lanelet_id, 34576); EXPECT_EQ( - canonicalized_lanelet_pose.value().s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34570) + - hdmap_utils.getLaneletLength(34576)); + canonicalized_lanelet_pose.value().s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34570) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34576)); } /** @@ -368,15 +394,16 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeNegative) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizePositive) { double non_canonicalized_lanelet_s = 30.0; - const auto canonicalized_lanelet_pose = - std::get>(hdmap_utils.canonicalizeLaneletPose( + const auto canonicalized_lanelet_pose = std::get>( + traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose( traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s))); EXPECT_EQ(canonicalized_lanelet_pose.value().lanelet_id, 34579); EXPECT_EQ( - canonicalized_lanelet_pose.value().s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34585) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_pose.value().s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34585) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); } /** @@ -387,8 +414,8 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizePositive) TEST_F(HdMapUtilsTest_StandardMap, Canonicalize) { const double non_canonicalized_lanelet_s = 2.0; - const auto canonicalized_lanelet_pose = - std::get>(hdmap_utils.canonicalizeLaneletPose( + const auto canonicalized_lanelet_pose = std::get>( + traffic_simulator::lanelet_wrapper::pose::canonicalizeLaneletPose( traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s))); EXPECT_EQ(canonicalized_lanelet_pose.value().lanelet_id, 34981); @@ -409,25 +436,29 @@ TEST_F(HdMapUtilsTest_StandardMap, Canonicalize) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllNegative) { const double non_canonicalized_lanelet_s = -22.0; - const auto canonicalized_lanelet_poses = hdmap_utils.getAllCanonicalizedLaneletPoses( - traffic_simulator::helper::constructLaneletPose(34564, non_canonicalized_lanelet_s)); + const auto canonicalized_lanelet_poses = + traffic_simulator::lanelet_wrapper::pose::alternativeLaneletPoses( + traffic_simulator::helper::constructLaneletPose(34564, non_canonicalized_lanelet_s)); EXPECT_EQ(canonicalized_lanelet_poses.size(), static_cast(3)); EXPECT_EQ(canonicalized_lanelet_poses[0].lanelet_id, 34576); EXPECT_EQ( - canonicalized_lanelet_poses[0].s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34570) + - hdmap_utils.getLaneletLength(34576)); + canonicalized_lanelet_poses[0].s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34570) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34576)); EXPECT_EQ(canonicalized_lanelet_poses[1].lanelet_id, 34981); EXPECT_EQ( - canonicalized_lanelet_poses[1].s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34636) + - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[1].s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34636) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); EXPECT_EQ(canonicalized_lanelet_poses[2].lanelet_id, 34600); EXPECT_EQ( - canonicalized_lanelet_poses[2].s, non_canonicalized_lanelet_s + - hdmap_utils.getLaneletLength(34648) + - hdmap_utils.getLaneletLength(34600)); + canonicalized_lanelet_poses[2].s, + non_canonicalized_lanelet_s + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34648) + + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34600)); } /** @@ -444,25 +475,29 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllNegative) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllPositive) { const double non_canonicalized_lanelet_s = 30.0; - const auto canonicalized_lanelet_poses = hdmap_utils.getAllCanonicalizedLaneletPoses( - traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); + const auto canonicalized_lanelet_poses = + traffic_simulator::lanelet_wrapper::pose::alternativeLaneletPoses( + traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); EXPECT_EQ(canonicalized_lanelet_poses.size(), static_cast(3)); EXPECT_EQ(canonicalized_lanelet_poses[0].lanelet_id, 34579); EXPECT_EQ( - canonicalized_lanelet_poses[0].s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34585) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[0].s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34585) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); EXPECT_EQ(canonicalized_lanelet_poses[1].lanelet_id, 34564); EXPECT_EQ( - canonicalized_lanelet_poses[1].s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34636) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[1].s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34636) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); EXPECT_EQ(canonicalized_lanelet_poses[2].lanelet_id, 34630); EXPECT_EQ( - canonicalized_lanelet_poses[2].s, non_canonicalized_lanelet_s - - hdmap_utils.getLaneletLength(34651) - - hdmap_utils.getLaneletLength(34981)); + canonicalized_lanelet_poses[2].s, + non_canonicalized_lanelet_s - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34651) - + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34981)); } /** @@ -473,8 +508,9 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAllPositive) TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAll) { const double non_canonicalized_lanelet_s = 2.0; - const auto canonicalized_lanelet_poses = hdmap_utils.getAllCanonicalizedLaneletPoses( - traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); + const auto canonicalized_lanelet_poses = + traffic_simulator::lanelet_wrapper::pose::alternativeLaneletPoses( + traffic_simulator::helper::constructLaneletPose(34981, non_canonicalized_lanelet_s)); EXPECT_EQ(canonicalized_lanelet_poses.size(), static_cast(1)); EXPECT_EQ(canonicalized_lanelet_poses[0].lanelet_id, 34981); @@ -664,7 +700,7 @@ TEST_F(HdMapUtilsTest_StandardMap, getCollisionPointInLaneCoordinate_invalidCros */ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_correct) { - const auto lanelet_pose = hdmap_utils.toLaneletPose( + const auto lanelet_pose = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), false); // angle to make pose aligned with the lanelet @@ -702,7 +738,7 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_negativeOffset) 73757.0 + std::sin(offset_yaw) * std::abs(offset)), makeQuaternionFromYaw(yaw)); - const auto lanelet_pose = hdmap_utils.toLaneletPose(pose, false); + const auto lanelet_pose = traffic_simulator::lanelet_wrapper::pose::toLaneletPose(pose, false); const auto reference_lanelet_pose = traffic_simulator_msgs::build() @@ -724,9 +760,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_negativeOffset) TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_reverse) { EXPECT_FALSE( - hdmap_utils - .toLaneletPose( - makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI_2 + M_PI_2 / 3.0)), false) + traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI_2 + M_PI_2 / 3.0)), false) .has_value()); // angle to make pose reverse aligned with the lanelet } @@ -737,11 +772,9 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_reverse) TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_notOnLanelet) { EXPECT_FALSE( - hdmap_utils - .toLaneletPose( - makePose( - makePoint(3790.0 + 5.0, 73757.0 - 5.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), - true) + traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3790.0 + 5.0, 73757.0 - 5.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), + true) .has_value()); // angle to make pose aligned with the lanelet } @@ -751,10 +784,9 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_notOnLanelet) */ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_empty) { - EXPECT_FALSE(hdmap_utils - .toLaneletPose( - makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), - lanelet::Ids{}) + EXPECT_FALSE(traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3790.0, 73757.0), makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), + lanelet::Ids{}) .has_value()); // angle to make pose aligned with the lanelet } @@ -771,13 +803,11 @@ TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_empty) TEST_F(HdMapUtilsTest_StandardMap, toLaneletPose_boundingBoxMatchPrevious) { EXPECT_LANELET_POSE_NEAR( - hdmap_utils - .toLaneletPose( - makePose( - makePoint(3774.9, 73749.2), - makeQuaternionFromYaw( - M_PI + M_PI_2 / 3.0)), // angle to make pose aligned with the lanelet - makeBoundingBox(), false, 0.5) + traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose( + makePoint(3774.9, 73749.2), + makeQuaternionFromYaw(M_PI + M_PI_2 / 3.0)), // angle to make pose aligned with the lanelet + makeBoundingBox(), false, 0.5) .value(), traffic_simulator_msgs::build() .lanelet_id(34600) @@ -903,7 +933,8 @@ TEST_F(HdMapUtilsTest_EmptyMap, getClosestLaneletId_emptyMap) */ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds) { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(34468); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(34468); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(120660)); @@ -917,7 +948,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds) */ TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getPreviousLaneletIds_RoadShoulder) { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(34768); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(34768); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34696)); @@ -932,7 +964,7 @@ TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getPreviousLaneletIds_RoadShoulder) TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds_multiplePrevious) { lanelet::Ids prev_lanelets = {34411, 34465}; - auto result_ids = hdmap_utils.getPreviousLaneletIds(34462); + auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(34462); std::sort(prev_lanelets.begin(), prev_lanelets.end()); std::sort(result_ids.begin(), result_ids.end()); @@ -954,14 +986,16 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds_direction) const lanelet::Id prev_lanelet_straight = 34465; { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(curr_lanelet, "left"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(curr_lanelet, "left"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(prev_lanelet_left)); } } { - const auto result_ids = hdmap_utils.getPreviousLaneletIds(curr_lanelet, "straight"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::previousLaneletIds(curr_lanelet, "straight"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(prev_lanelet_straight)); @@ -974,9 +1008,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLaneletIds_direction) * Test next lanelets id obtaining correctness * with a lanelet that has a lanelet following it. */ -TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds) +TEST_F(HdMapUtilsTest_StandardMap, nextLaneletIds) { - const auto result_ids = hdmap_utils.getNextLaneletIds(120660); + const auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(120660); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34468)); @@ -988,9 +1022,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds) * Test next lanelets id obtaining correctness * with a lanelet that has a lanelet following it and is a shoulder lane. */ -TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getNextLaneletIds_RoadShoulder) +TEST_F(HdMapUtilsTest_WithRoadShoulderMap, nextLaneletIds_RoadShoulder) { - const auto result_ids = hdmap_utils.getNextLaneletIds(34696); + const auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(34696); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34768)); @@ -1002,10 +1036,10 @@ TEST_F(HdMapUtilsTest_WithRoadShoulderMap, getNextLaneletIds_RoadShoulder) * Test next lanelets id obtaining correctness * with a lanelet that has several lanelets following it. */ -TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds_multipleNext) +TEST_F(HdMapUtilsTest_StandardMap, nextLaneletIds_multipleNext) { lanelet::Ids next_lanelets = {34438, 34465}; - auto result_ids = hdmap_utils.getNextLaneletIds(34468); + auto result_ids = traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(34468); std::sort(next_lanelets.begin(), next_lanelets.end()); std::sort(result_ids.begin(), result_ids.end()); @@ -1020,19 +1054,21 @@ TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds_multipleNext) * - the goal is to test the function specialization that takes a direction as an argument * and returns only the next lanelets that have this turn direction. */ -TEST_F(HdMapUtilsTest_StandardMap, getNextLaneletIds_direction) +TEST_F(HdMapUtilsTest_StandardMap, nextLaneletIds_direction) { const lanelet::Id curr_lanelet = 34468; { - const auto result_ids = hdmap_utils.getNextLaneletIds(curr_lanelet, "left"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(curr_lanelet, "left"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34438)); } } { - const auto result_ids = hdmap_utils.getNextLaneletIds(curr_lanelet, "straight"); + const auto result_ids = + traffic_simulator::lanelet_wrapper::lanelet_map::nextLaneletIds(curr_lanelet, "straight"); EXPECT_EQ(result_ids.size(), static_cast(1)); if (result_ids.size() == 1) { EXPECT_EQ(result_ids[0], static_cast(34465)); @@ -1086,7 +1122,8 @@ TEST_F(HdMapUtilsTest_StandardMap, isInLanelet_correct) TEST_F(HdMapUtilsTest_StandardMap, isInLanelet_after) { const lanelet::Id lanelet_id = 34696; - EXPECT_FALSE(hdmap_utils.isInLanelet(lanelet_id, hdmap_utils.getLaneletLength(lanelet_id) + 5.0)); + EXPECT_FALSE(hdmap_utils.isInLanelet( + lanelet_id, traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id) + 5.0)); } /** @@ -1135,7 +1172,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPoints_sLargerThanLaneletLength) { const lanelet::Id lanelet_id = 34696; - const auto lanelet_length = hdmap_utils.getLaneletLength(lanelet_id); + const auto lanelet_length = + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); const auto points = hdmap_utils.toMapPoints( lanelet_id, std::vector{lanelet_length + 10.0, lanelet_length + 20.0, lanelet_length + 30.0}); @@ -1166,8 +1204,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPoints_empty) */ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_onlyOffset) { - const auto map_pose = - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34696, 10.0, 0.5)); + const auto map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34696, 10.0, 0.5)); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); EXPECT_POSE_NEAR( @@ -1182,7 +1220,7 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_onlyOffset) */ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_additionalRotation) { - const auto map_pose = hdmap_utils.toMapPose( + const auto map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( traffic_simulator::helper::constructLaneletPose(34696, 10.0, 0.0, 0.0, 0.0, M_PI_4)); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); @@ -1198,8 +1236,8 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_negativeS) { geometry_msgs::msg::PoseStamped map_pose; EXPECT_NO_THROW( - map_pose = - hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose(34696, -10.0))); + map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose(34696, -10.0))); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); EXPECT_POSE_NEAR( @@ -1215,8 +1253,10 @@ TEST_F(HdMapUtilsTest_StandardMap, toMapPose_sLargerThanLaneletLength) geometry_msgs::msg::PoseStamped map_pose; EXPECT_NO_THROW( - map_pose = hdmap_utils.toMapPose(traffic_simulator::helper::constructLaneletPose( - lanelet_id, hdmap_utils.getLaneletLength(lanelet_id) + 10.0))); + map_pose = traffic_simulator::lanelet_wrapper::pose::toMapPose( + traffic_simulator::helper::constructLaneletPose( + lanelet_id, + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id) + 10.0))); EXPECT_STREQ(map_pose.header.frame_id.c_str(), "map"); EXPECT_POSE_NEAR( @@ -1977,7 +2017,7 @@ TEST_F(HdMapUtilsTest_StandardMap, isTrafficLightRegulatoryElement_invalidId) */ TEST_F(HdMapUtilsTest_StandardMap, getLaneletLength_simple) { - EXPECT_NEAR(hdmap_utils.getLaneletLength(34468), 55.5, 1.0); + EXPECT_NEAR(traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(34468), 55.5, 1.0); } /** @@ -1989,7 +2029,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getLaneletLength_cache) { const lanelet::Id id = 34468; - EXPECT_EQ(hdmap_utils.getLaneletLength(id), hdmap_utils.getLaneletLength(id)); + EXPECT_EQ( + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(id), + traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(id)); } /** @@ -2036,9 +2078,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getTrafficLightIdsOnPath_empty) */ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLanelet) { - const auto pose_from = hdmap_utils.toLaneletPose( + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3812.65, 73810.13, -2.80), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); - const auto pose_to = hdmap_utils.toLaneletPose( + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3825.10, 73786.34, -1.82), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); @@ -2057,9 +2099,9 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLanelet) */ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLaneletBehind) { - const auto pose_to = hdmap_utils.toLaneletPose( + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3812.65, 73810.13, -2.80), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); - const auto pose_from = hdmap_utils.toLaneletPose( + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(3825.10, 73786.34, -1.82), makeQuaternionFromYaw(90.0)), lanelet::Id{34606}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); @@ -2076,10 +2118,10 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLaneletBehind) */ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) { - const auto pose_from = - hdmap_utils.toLaneletPose(makePose(makePoint(3801.19, 73812.70, -2.86)), lanelet::Id{120660}); - const auto pose_to = - hdmap_utils.toLaneletPose(makePose(makePoint(3724.70, 73773.00, -1.20)), lanelet::Id{34462}); + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3801.19, 73812.70, -2.86)), lanelet::Id{120660}); + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( + makePose(makePoint(3724.70, 73773.00, -1.20)), lanelet::Id{34462}); ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); @@ -2097,10 +2139,10 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) */ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLaneletNoRoute) { - const auto pose_to = hdmap_utils.toLaneletPose( + const auto pose_to = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(81590.79, 50067.66, 35.0), makeQuaternionFromYaw(90.0)), lanelet::Id{3002185}); - const auto pose_from = hdmap_utils.toLaneletPose( + const auto pose_from = traffic_simulator::lanelet_wrapper::pose::toLaneletPose( makePose(makePoint(81596.20, 50068.04, 35.0), makeQuaternionFromYaw(90.0)), lanelet::Id{3002166}); ASSERT_TRUE(pose_from.has_value()); diff --git a/simulation/traffic_simulator/test/src/helper_functions.hpp b/simulation/traffic_simulator/test/src/helper_functions.hpp index c10cc1d4931..796ae9f1024 100644 --- a/simulation/traffic_simulator/test/src/helper_functions.hpp +++ b/simulation/traffic_simulator/test/src/helper_functions.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -96,17 +97,22 @@ auto makeHdMapUtilsSharedPointer() -> std::shared_ptr .altitude(0.0)); } +auto activateLaneletWrapper(const std::string map_name) -> void +{ + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/" + map_name + "/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); +} + auto makeCanonicalizedLaneletPose( - std::shared_ptr hdmap_utils, const lanelet::Id id = 120659, - const double s = 0.0, const double offset = 0.0) + const lanelet::Id id = 120659, const double s = 0.0, const double offset = 0.0) -> traffic_simulator::lanelet_pose::CanonicalizedLaneletPose { return traffic_simulator::lanelet_pose::CanonicalizedLaneletPose( - traffic_simulator::helper::constructLaneletPose(id, s, offset), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(id, s, offset)); } auto makeEntityStatus( - std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, const std::string name = "default_entity_name", @@ -121,15 +127,14 @@ auto makeEntityStatus( .name(name) .bounding_box(bbox) .action_status(traffic_simulator::helper::constructActionStatus(speed, 0.0, 0.0, 0.0)) - .pose(hdmap_utils->toMapPose(static_cast(pose)).pose) + .pose(traffic_simulator::pose::toMapPose(pose)) .lanelet_pose(static_cast(pose)) .lanelet_pose_valid(true); } auto makeEntityStatus( - std::shared_ptr /* hdmap_utils */, geometry_msgs::msg::Pose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, - const std::string name = "default_entity_name", + geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, + const double speed = 0.0, const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::EntityStatus { @@ -147,7 +152,6 @@ auto makeEntityStatus( } auto makeCanonicalizedEntityStatus( - std::shared_ptr hdmap_utils, traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_lanelet_pose, traffic_simulator_msgs::msg::BoundingBox bbox, const double speed = 0.0, const std::string name = "default_entity_name", @@ -155,14 +159,14 @@ auto makeCanonicalizedEntityStatus( -> traffic_simulator::entity_status::CanonicalizedEntityStatus { return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, canonicalized_lanelet_pose, bbox, speed, name, type), + makeEntityStatus(canonicalized_lanelet_pose, bbox, speed, name, type), canonicalized_lanelet_pose); } auto makeCanonicalizedEntityStatus( - std::shared_ptr hdmap_utils, geometry_msgs::msg::Pose pose, - traffic_simulator_msgs::msg::BoundingBox bbox, const double matching_distance = 1.0, - const double speed = 0.0, const std::string name = "default_entity_name", + geometry_msgs::msg::Pose pose, traffic_simulator_msgs::msg::BoundingBox bbox, + const double matching_distance = 1.0, const double speed = 0.0, + const std::string name = "default_entity_name", const uint8_t type = traffic_simulator_msgs::msg::EntityType::VEHICLE) -> traffic_simulator::entity_status::CanonicalizedEntityStatus { @@ -170,9 +174,9 @@ auto makeCanonicalizedEntityStatus( (traffic_simulator_msgs::msg::EntityType::PEDESTRIAN == type || traffic_simulator_msgs::msg::EntityType::MISC_OBJECT == type); const auto canonicalized_lanelet_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, bbox, include_crosswalk, matching_distance, hdmap_utils); + pose, bbox, include_crosswalk, matching_distance); return traffic_simulator::entity_status::CanonicalizedEntityStatus( - makeEntityStatus(hdmap_utils, pose, bbox, speed, name, type), canonicalized_lanelet_pose); + makeEntityStatus(pose, bbox, speed, name, type), canonicalized_lanelet_pose); } #endif // TRAFFIC_SIMULATOR__TEST__ENTITY_HELPER_FUNCTIONS_HPP_ diff --git a/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp b/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp index 0f95927fc1e..6d39e8f1e90 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/common_test_fixtures.hpp @@ -19,6 +19,7 @@ #include #include +#include constexpr char architecture_old[] = "awf/universe/20230906"; constexpr char architecture_new[] = "awf/universe/20240605"; @@ -40,6 +41,10 @@ class TrafficLightsInternalTestArchitectureDependent : public testing::Test std::is_same_v or std::is_same_v, "Given TrafficLights type is not supported"); + + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); } const lanelet::Id id{34836}; diff --git a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp index a85c91a2c75..7709ec5a522 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/helper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// 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. diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp index 36ff6baf51a..e2522ac5428 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_light.cpp @@ -19,6 +19,7 @@ #include #include #include +#include using TrafficLight = traffic_simulator::TrafficLight; using Color = TrafficLight::Color; @@ -724,6 +725,9 @@ class TrafficLightTest : public testing::Test .longitude(139.78066608243) .altitude(0.0)) { + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); } hdmap_utils::HdMapUtils map_manager; }; diff --git a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp index ec5d0bc7beb..be1c9ce70b1 100644 --- a/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp +++ b/simulation/traffic_simulator/test/src/traffic_lights/test_traffic_lights.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. All rights reserved. +// 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. @@ -19,6 +19,7 @@ #include #include #include +#include #include "../expect_eq_macros.hpp" #include "helper.hpp" @@ -31,7 +32,12 @@ using namespace std::chrono_literals; class TrafficLightsTest : public testing::Test { public: - TrafficLightsTest() = default; + TrafficLightsTest() + { + const auto lanelet_path = ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/standard_map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(lanelet_path); + } const lanelet::Id id{34836}; diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 8dc5b5cdee8..6eaa2e00170 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -43,6 +43,7 @@ class distanceTest_FourTrackHighwayMap : public testing::Test .longitude(138.8024583466017) .altitude(0.0))) { + activateLaneletWrapper("four_track_highway"); } std::shared_ptr hdmap_utils_ptr; }; @@ -59,6 +60,7 @@ class distanceTest_StandardMap : public testing::Test .longitude(139.78066608243) .altitude(0.0))) { + activateLaneletWrapper("standard_map"); } std::shared_ptr hdmap_utils_ptr; }; @@ -75,6 +77,7 @@ class distanceTest_IntersectionMap : public testing::Test .longitude(139.74821144562) .altitude(0.0))) { + activateLaneletWrapper("intersection"); } std::shared_ptr hdmap_utils_ptr; }; @@ -86,10 +89,9 @@ class distanceTest_IntersectionMap : public testing::Test */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); { const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), @@ -110,10 +112,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(201L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(201L, 0.0, 0.0); { const auto result = traffic_simulator::distance::lateralDistance( pose_from, pose_to, std::numeric_limits::infinity(), @@ -137,9 +138,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) { const auto pose_from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -164,10 +165,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); constexpr double approx_distance = -3.0; constexpr double tolerance = 0.5; { @@ -196,9 +196,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) { const auto pose_from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); + traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); + const auto pose_to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -214,10 +214,9 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) */ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_matching) { - const auto pose_from = traffic_simulator::helper::constructCanonicalizedLaneletPose( - 3002184L, 0.0, 0.0, hdmap_utils_ptr); - const auto pose_to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002184L, 0.0, 0.0); + const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0); { traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -238,10 +237,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.44, 50006.09, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81595.44, 50006.09, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81584.48, 50084.76, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81584.48, 50084.76, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -259,11 +258,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noChange) { { - const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3800.05, 73715.77, 0.5, 30.0), false, hdmap_utils_ptr); + const auto pose_from = + traffic_simulator::toCanonicalizedLaneletPose(makePose(3800.05, 73715.77, 0.5, 30.0), false); ASSERT_TRUE(pose_from.has_value()); - const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(3841.26, 73748.80, 0.5, 110.0), false, hdmap_utils_ptr); + const auto pose_to = + traffic_simulator::toCanonicalizedLaneletPose(makePose(3841.26, 73748.80, 0.5, 110.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -283,10 +282,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81585.79, 50042.62, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81585.79, 50042.62, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81588.34, 50083.23, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81588.34, 50083.23, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -305,10 +304,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.02, 50065.76, 35.0, 280.0), false, hdmap_utils_ptr); + makePose(81599.02, 50065.76, 35.0, 280.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.61, 50045.16, 35.0, 280.0), false, hdmap_utils_ptr); + makePose(81599.61, 50045.16, 35.0, 280.0), false); ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( @@ -328,10 +327,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81595.47, 49982.80, 36.0, 100.0), false, hdmap_utils_ptr); + makePose(81595.47, 49982.80, 36.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81599.34, 50022.34, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81599.34, 50022.34, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -343,10 +342,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.35, 50015.63, 35.0, 280.0), false, hdmap_utils_ptr); + makePose(81612.35, 50015.63, 35.0, 280.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81612.95, 49991.30, 35.5, 280.0), false, hdmap_utils_ptr); + makePose(81612.95, 49991.30, 35.5, 280.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -367,10 +366,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81592.96, 49997.94, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81592.96, 49997.94, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81570.56, 50141.75, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81570.56, 50141.75, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -383,10 +382,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81587.31, 50165.57, 35.0, 100.0), false, hdmap_utils_ptr); + makePose(81587.31, 50165.57, 35.0, 100.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(81610.25, 49988.59, 35.5, 100.0), false, hdmap_utils_ptr); + makePose(81610.25, 49988.59, 35.5, 100.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -406,10 +405,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86627.71, 44972.06, 3.0, 340.0), false, hdmap_utils_ptr); + makePose(86627.71, 44972.06, 3.0, 340.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -421,10 +420,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86555.38, 45000.88, 3.0, 340.0), false, hdmap_utils_ptr); + makePose(86555.38, 45000.88, 3.0, 340.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86647.23, 44882.51, 3.0, 240.0), false, hdmap_utils_ptr); + makePose(86647.23, 44882.51, 3.0, 240.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -436,10 +435,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86553.48, 44990.56, 3.0, 150.0), false, hdmap_utils_ptr); + makePose(86553.48, 44990.56, 3.0, 150.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -451,10 +450,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86788.82, 44993.77, 3.0, 210.0), false, hdmap_utils_ptr); + makePose(86788.82, 44993.77, 3.0, 210.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86579.91, 44979.00, 3.0, 150.0), false, hdmap_utils_ptr); + makePose(86579.91, 44979.00, 3.0, 150.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -475,10 +474,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86736.13, 44969.63, 3.0, 210.0), false, hdmap_utils_ptr); + makePose(86736.13, 44969.63, 3.0, 210.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86642.95, 44958.78, 3.0, 340.0), false, hdmap_utils_ptr); + makePose(86642.95, 44958.78, 3.0, 340.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -490,10 +489,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86732.06, 44976.58, 3.0, 210.0), false, hdmap_utils_ptr); + makePose(86732.06, 44976.58, 3.0, 210.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86704.59, 44927.32, 3.0, 340.0), false, hdmap_utils_ptr); + makePose(86704.59, 44927.32, 3.0, 340.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; @@ -514,9 +513,9 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch { { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86637.19, 44967.35, 3.0, 340.0), false, hdmap_utils_ptr); + makePose(86637.19, 44967.35, 3.0, 340.0), false); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86648.82, 44886.19, 3.0, 240.0), false, hdmap_utils_ptr); + makePose(86648.82, 44886.19, 3.0, 240.0), false); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -528,10 +527,10 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch } { const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86719.94, 44957.20, 3.0, 210.0), false, hdmap_utils_ptr); + makePose(86719.94, 44957.20, 3.0, 210.0), false); ASSERT_TRUE(pose_from.has_value()); const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( - makePose(86599.32, 44975.01, 3.0, 180.0), false, hdmap_utils_ptr); + makePose(86599.32, 44975.01, 3.0, 180.0), false); ASSERT_TRUE(pose_from.has_value()); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index cde4cce5e8e..b2aa0b52244 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -36,6 +36,7 @@ class PoseTest : public testing::Test .longitude(138.8024583466017) .altitude(0.0))) { + activateLaneletWrapper("four_track_highway"); } std::shared_ptr hdmap_utils; @@ -79,7 +80,7 @@ TEST(pose, quietNaNLaneletPose) TEST_F(PoseTest, canonicalize_default) { const auto pose = - traffic_simulator::pose::canonicalize(traffic_simulator_msgs::msg::LaneletPose(), hdmap_utils); + traffic_simulator::pose::toCanonicalizedLaneletPose(traffic_simulator_msgs::msg::LaneletPose()); EXPECT_FALSE(pose.has_value()); } @@ -90,11 +91,11 @@ TEST_F(PoseTest, canonicalize_default) TEST_F(PoseTest, canonicalize_invalid) { EXPECT_THROW( - traffic_simulator::pose::canonicalize( - traffic_simulator::pose::quietNaNLaneletPose(), hdmap_utils), + traffic_simulator::pose::toCanonicalizedLaneletPose( + traffic_simulator::pose::quietNaNLaneletPose()), std::runtime_error); - EXPECT_FALSE(traffic_simulator::pose::canonicalize( - traffic_simulator::helper::constructLaneletPose(203, 1000.0, 0.0), hdmap_utils)); + EXPECT_FALSE(traffic_simulator::pose::toCanonicalizedLaneletPose( + traffic_simulator::helper::constructLaneletPose(203, 1000.0, 0.0))); } /** @@ -105,7 +106,7 @@ TEST_F(PoseTest, canonicalize_valid) const auto pose = traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0); std::optional canonicalized_pose; - EXPECT_NO_THROW(canonicalized_pose = traffic_simulator::pose::canonicalize(pose, hdmap_utils)); + EXPECT_NO_THROW(canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose(pose)); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_LANELET_POSE_EQ( static_cast(canonicalized_pose.value()), pose); @@ -117,7 +118,7 @@ TEST_F(PoseTest, canonicalize_valid) TEST_F(PoseTest, toMapPose_CanonicalizedLaneletPose) { const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_pose( - traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0), hdmap_utils); + traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0)); const geometry_msgs::msg::Pose pose = makePose( makePoint(81585.1622, 50176.9202, 34.2595), @@ -137,7 +138,7 @@ TEST_F(PoseTest, toMapPose_LaneletPose) makePoint(81585.1622, 50176.9202, 34.2595), geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); - EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(lanelet_pose, hdmap_utils), pose, 0.01); + EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(lanelet_pose), pose, 0.01); } /** @@ -146,12 +147,11 @@ TEST_F(PoseTest, toMapPose_LaneletPose) TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); - const auto canonicalized_pose = - traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils); + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -167,8 +167,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) { const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); - EXPECT_EQ( - traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils), std::nullopt); + EXPECT_EQ(traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true), std::nullopt); } /** @@ -177,12 +176,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); - const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), true, 1.0, hdmap_utils); + const auto canonicalized_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, makeBoundingBox(), true, 1.0); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -197,13 +196,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); EXPECT_EQ( - traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeSmallBoundingBox(), true, 0.0, hdmap_utils), + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, makeSmallBoundingBox(), true, 0.0), std::nullopt); } @@ -216,7 +214,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) EXPECT_EQ( traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils), + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0), std::nullopt); } @@ -226,12 +224,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils); + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -248,8 +246,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); EXPECT_EQ( - traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), {195}, true, 1.0, hdmap_utils), + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, makeBoundingBox(), {195}, true, 1.0), std::nullopt); } @@ -259,12 +256,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid) { const auto lanelet_pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), lanelet::Ids{195}, true, 1.0, hdmap_utils); + pose, makeBoundingBox(), lanelet::Ids{195}, true, 1.0); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -340,8 +337,7 @@ TEST_F(PoseTest, relativePose_canonicalized_end_position) const auto from = makePose( makePoint(81585.1622, 50176.9202, 34.2595), geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0); const auto relative = traffic_simulator::pose::relativePose(from, to); @@ -357,8 +353,7 @@ TEST_F(PoseTest, relativePose_canonicalized_start_position) const auto pose_relative = makePose( makePoint(145244.7916, 786706.3326, 0.0127), geometry_msgs::build().x(0).y(0).z(0).w(1)); - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); const auto to = makePose( makePoint(881586.9767, 50167.0862, 34.2722), geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); @@ -420,10 +415,8 @@ TEST_F(PoseTest, boundingBoxRelativePose_intersect) */ TEST_F(PoseTest, relativeLaneletPose_s_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0); const auto relative = traffic_simulator::pose::relativeLaneletPose( from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); @@ -436,10 +429,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_invalid) */ TEST_F(PoseTest, relativeLaneletPose_s_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0); const auto relative = traffic_simulator::pose::relativeLaneletPose( from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); @@ -452,10 +443,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_valid) */ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0); const auto relative = traffic_simulator::pose::relativeLaneletPose( from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); @@ -468,10 +457,8 @@ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) */ TEST_F(PoseTest, relativeLaneletPose_offset_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0); traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; lane_changeable_routing_configuration.allow_lane_change = true; @@ -486,10 +473,8 @@ TEST_F(PoseTest, relativeLaneletPose_offset_valid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -503,10 +488,8 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -520,10 +503,8 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -537,10 +518,8 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) { - const auto from = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0, hdmap_utils); - const auto to = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + const auto from = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0); + const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0); const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( @@ -554,8 +533,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) */ TEST_F(PoseTest, isInLanelet_inside) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0); EXPECT_TRUE(traffic_simulator::pose::isInLanelet( pose, 195, std::numeric_limits::epsilon(), hdmap_utils)); @@ -567,7 +545,7 @@ TEST_F(PoseTest, isInLanelet_inside) TEST_F(PoseTest, isInLanelet_outsideFrontFar) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1.0, hdmap_utils)); } @@ -578,7 +556,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontFar) TEST_F(PoseTest, isInLanelet_outsideFrontClose) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0); EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2.0, hdmap_utils)); } @@ -588,8 +566,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontClose) */ TEST_F(PoseTest, isInLanelet_outsideBackFar) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 195, 2, hdmap_utils)); } @@ -599,8 +576,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackFar) */ TEST_F(PoseTest, isInLanelet_outsideBackClose) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0); EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10.0, hdmap_utils)); } @@ -611,7 +587,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackClose) TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002171, 31.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002171, 31.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -621,8 +597,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) */ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 5.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 5.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -633,7 +608,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) { const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 20.0, 0.0, hdmap_utils); + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 20.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -643,8 +618,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) */ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 5.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 5.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -654,8 +628,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) */ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) { - const auto pose = - traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0); EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); } @@ -666,7 +639,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) TEST_F(PoseTest, laneletLength_invalid) { EXPECT_THROW( - traffic_simulator::pose::laneletLength(10000000000000000, hdmap_utils), std::runtime_error); + traffic_simulator::lanelet_map::laneletLength(10000000000000000), std::runtime_error); } /** @@ -674,5 +647,5 @@ TEST_F(PoseTest, laneletLength_invalid) */ TEST_F(PoseTest, laneletLength_valid) { - EXPECT_NEAR(traffic_simulator::pose::laneletLength(195, hdmap_utils), 107.74, 0.01); + EXPECT_NEAR(traffic_simulator::lanelet_map::laneletLength(195), 107.74, 0.01); } diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 77be0a39e36..6541df1ea0a 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,21 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index bea2365d6ac..f02e34f2199 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 7.4.7 + 8.0.0 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index f7f7152ea39..db1d1a77efb 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,26 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge pull request `#1472 `_ from tier4/ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose + HdMapUtils refactor (PR 1/6) - create lanelet_wrapper: use ::lanelet_map and ::pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* ref(traffic_simulator): improve Configuration, traffic_rules, lanelet_wrapper +* feat(traffic_simulator, behavior_tree_plugin): use lanelet_wrapper::pose in parts previously overlooked +* feat(traffic_simulator, random_test_runner): adapt tests for using pose:: from lanelet_wrapper +* Contributors: Dawid Moszynski, Dawid Moszyński, Masaya Kataoka, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp index a33b62fb37c..fa426092b46 100644 --- a/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/test_executor.hpp @@ -78,8 +78,9 @@ class TestExecutor RCLCPP_INFO_STREAM(logger_, message); scenario_completed_ = false; - if (const auto ego_start_canonicalized_lanelet_pose = traffic_simulator::pose::canonicalize( - test_description_.ego_start_position, api_->getHdmapUtils()); + if (const auto ego_start_canonicalized_lanelet_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose( + test_description_.ego_start_position); !ego_start_canonicalized_lanelet_pose) { throw std::runtime_error( "Can not canonicalize ego start lanelet pose: id: " + @@ -135,8 +136,8 @@ class TestExecutor for (size_t i = 0; i < test_description_.npcs_descriptions.size(); i++) { const auto & npc_descr = test_description_.npcs_descriptions[i]; - if (const auto npc_start_canonicalized_lanelet_pose = traffic_simulator::pose::canonicalize( - npc_descr.start_position, api_->getHdmapUtils()); + if (const auto npc_start_canonicalized_lanelet_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose(npc_descr.start_position); !npc_start_canonicalized_lanelet_pose) { throw std::runtime_error( "Can not canonicalize npc start lanelet pose: id: " + diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 4a893f77a61..a6075603dac 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 7.4.7 + 8.0.0 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp index e228a0e7bb9..09e56a4d1b8 100644 --- a/test_runner/random_test_runner/src/lanelet_utils.cpp +++ b/test_runner/random_test_runner/src/lanelet_utils.cpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include LaneletUtils::LaneletUtils(const boost::filesystem::path & filename) { @@ -45,6 +47,7 @@ LaneletUtils::LaneletUtils(const boost::filesystem::path & filename) hdmap_utils_ptr_ = std::make_shared(filename, geographic_msgs::msg::GeoPoint()); + traffic_simulator::lanelet_map::activate(filename.string()); } std::vector LaneletUtils::getLaneletIds() const @@ -55,7 +58,7 @@ std::vector LaneletUtils::getLaneletIds() const geometry_msgs::msg::PoseStamped LaneletUtils::toMapPose( const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch) const { - return hdmap_utils_ptr_->toMapPose(lanelet_pose, fill_pitch); + return traffic_simulator::lanelet_wrapper::pose::toMapPose(lanelet_pose, fill_pitch); } std::vector LaneletUtils::getRoute(int64_t from_lanelet_id, int64_t to_lanelet_id) @@ -65,15 +68,15 @@ std::vector LaneletUtils::getRoute(int64_t from_lanelet_id, int64_t to_ double LaneletUtils::getLaneletLength(int64_t lanelet_id) const { - return hdmap_utils_ptr_->getLaneletLength(lanelet_id); + return traffic_simulator::lanelet_wrapper::lanelet_map::laneletLength(lanelet_id); } double LaneletUtils::computeDistance( const traffic_simulator_msgs::msg::LaneletPose & p1, const traffic_simulator_msgs::msg::LaneletPose & p2) const { - auto p1_g = hdmap_utils_ptr_->toMapPose(p1).pose.position; - auto p2_g = hdmap_utils_ptr_->toMapPose(p2).pose.position; + auto p1_g = traffic_simulator::lanelet_wrapper::pose::toMapPose(p1).pose.position; + auto p2_g = traffic_simulator::lanelet_wrapper::pose::toMapPose(p2).pose.position; geometry_msgs::msg::Point d; d.x = p1_g.x - p2_g.x; d.y = p1_g.y - p2_g.y; @@ -139,7 +142,7 @@ std::optional LaneletUtils::getOpposit global_pose.position.y = opposite_lane_global_position.y; global_pose.position.z = opposite_lane_global_position.z; - return hdmap_utils_ptr_->toLaneletPose(global_pose, false); + return traffic_simulator::lanelet_wrapper::pose::toLaneletPose(global_pose, false, 1.0); } enum SearchDirection { FORWARD, BACKWARD, INVALID }; diff --git a/test_runner/random_test_runner/src/random_test_runner.cpp b/test_runner/random_test_runner/src/random_test_runner.cpp index 0a407dbf5bd..383c1b0fe03 100644 --- a/test_runner/random_test_runner/src/random_test_runner.cpp +++ b/test_runner/random_test_runner/src/random_test_runner.cpp @@ -67,10 +67,13 @@ RandomTestRunner::RandomTestRunner(const rclcpp::NodeOptions & option) std::string map_path = ament_index_cpp::get_package_share_directory(test_suite_params.map_name) + "/map"; + const std::set auto_sink_entity_types{ + traffic_simulator_msgs::msg::EntityType::VEHICLE, + traffic_simulator_msgs::msg::EntityType::PEDESTRIAN}; + message = fmt::format("Map path found: {}", map_path); RCLCPP_INFO_STREAM(get_logger(), message); - - traffic_simulator::Configuration configuration(map_path); + traffic_simulator::Configuration configuration(map_path, "", auto_sink_entity_types); configuration.simulator_host = test_control_parameters.simulator_host; auto lanelet_utils = std::make_shared(configuration.lanelet2_map_path()); diff --git a/test_runner/random_test_runner/src/test_randomizer.cpp b/test_runner/random_test_runner/src/test_randomizer.cpp index f47b7ab0746..5ad27362105 100644 --- a/test_runner/random_test_runner/src/test_randomizer.cpp +++ b/test_runner/random_test_runner/src/test_randomizer.cpp @@ -51,7 +51,7 @@ TestDescription TestRandomizer::generate() test_suite_parameters_.ego_goal_lanelet_id, test_suite_parameters_.ego_goal_s, test_suite_parameters_.ego_goal_partial_randomization, test_suite_parameters_.ego_goal_partial_randomization_distance); - ret.ego_goal_pose = lanelet_utils_->toMapPose(ret.ego_goal_position, false).pose; + ret.ego_goal_pose = traffic_simulator::pose::toMapPose(ret.ego_goal_position); std::vector lanelets_around_start = lanelet_utils_->getLanesWithinDistance( ret.ego_start_position, test_suite_parameters_.npc_min_spawn_distance_from_ego, diff --git a/test_runner/random_test_runner/test/test_test_executor.cpp b/test_runner/random_test_runner/test/test_test_executor.cpp index 04fcacd9ac4..6ec14ed5f82 100644 --- a/test_runner/random_test_runner/test/test_test_executor.cpp +++ b/test_runner/random_test_runner/test/test_test_executor.cpp @@ -17,6 +17,7 @@ #include #include +#include class MockFieldOperatorApplication { @@ -66,6 +67,13 @@ auto getTestDescription() -> TestDescription class MockTrafficSimulatorAPI { public: + MockTrafficSimulatorAPI() + { + const std::string path = + ament_index_cpp::get_package_share_directory("random_test_runner") + "/map/lanelet2_map.osm"; + traffic_simulator::lanelet_map::activate(path); + } + traffic_simulator::EntityStatus entity_status_; std::shared_ptr<::testing::StrictMock> field_operator_application_mock = diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index 8d9b268039f..8cc3ee025bf 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,21 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +8.0.0 (2025-01-24) +------------------ +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose' of github.com:tier4/scenario_simulator_v2 into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge branch 'master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Merge remote-tracking branch 'origin/master' into ref/RJD-1387-hdmap-utils-to-lanelet-wrapper-pose +* Contributors: Dawid Moszynski, Dawid Moszyński, Mateusz Palczuk + 7.4.7 (2025-01-20) ------------------ * Merge branch 'master' into RJD-1511/bug_fix diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 88444f2c61f..643ee8df5c7 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 7.4.7 + 8.0.0 scenario test runner package Tatsuya Yamasaki Apache License 2.0