diff --git a/common/math/arithmetic/CHANGELOG.rst b/common/math/arithmetic/CHANGELOG.rst index e8c5823f94d..cfff1dfc6c6 100644 --- a/common/math/arithmetic/CHANGELOG.rst +++ b/common/math/arithmetic/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package arithmetic * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/common/math/arithmetic/package.xml b/common/math/arithmetic/package.xml index 1062780acc0..2ccd6b2e050 100644 --- a/common/math/arithmetic/package.xml +++ b/common/math/arithmetic/package.xml @@ -2,7 +2,7 @@ arithmetic - 5.4.0 + 6.0.1 arithmetic library for scenario_simulator_v2 Tatsuya Yamasaki Apache License 2.0 diff --git a/common/math/geometry/CHANGELOG.rst b/common/math/geometry/CHANGELOG.rst index bd11e9c90fd..bc41e228e98 100644 --- a/common/math/geometry/CHANGELOG.rst +++ b/common/math/geometry/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package geometry * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/common/math/geometry/package.xml b/common/math/geometry/package.xml index acede870ef0..35fc4f622b8 100644 --- a/common/math/geometry/package.xml +++ b/common/math/geometry/package.xml @@ -2,7 +2,7 @@ geometry - 5.4.0 + 6.0.1 geometry math library for scenario_simulator_v2 application Masaya Kataoka Apache License 2.0 diff --git a/common/scenario_simulator_exception/CHANGELOG.rst b/common/scenario_simulator_exception/CHANGELOG.rst index 0beb5379def..88e06c50693 100644 --- a/common/scenario_simulator_exception/CHANGELOG.rst +++ b/common/scenario_simulator_exception/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package scenario_simulator_exception * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/common/scenario_simulator_exception/package.xml b/common/scenario_simulator_exception/package.xml index 03353047e38..ad71a843b5b 100644 --- a/common/scenario_simulator_exception/package.xml +++ b/common/scenario_simulator_exception/package.xml @@ -2,7 +2,7 @@ scenario_simulator_exception - 5.4.0 + 6.0.1 Exception types for scenario simulator Tatsuya Yamasaki Apache License 2.0 diff --git a/common/simple_junit/CHANGELOG.rst b/common/simple_junit/CHANGELOG.rst index bd418f85147..34ae614834a 100644 --- a/common/simple_junit/CHANGELOG.rst +++ b/common/simple_junit/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package junit_exporter * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/common/simple_junit/package.xml b/common/simple_junit/package.xml index eba86375a46..fcf861c1bea 100644 --- a/common/simple_junit/package.xml +++ b/common/simple_junit/package.xml @@ -2,7 +2,7 @@ simple_junit - 5.4.0 + 6.0.1 Lightweight JUnit library for ROS 2 Masaya Kataoka Tatsuya Yamasaki diff --git a/common/status_monitor/CHANGELOG.rst b/common/status_monitor/CHANGELOG.rst index 09e129e59cf..0a54899f5d0 100644 --- a/common/status_monitor/CHANGELOG.rst +++ b/common/status_monitor/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package status_monitor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/common/status_monitor/package.xml b/common/status_monitor/package.xml index badfc2788e4..f57c774637a 100644 --- a/common/status_monitor/package.xml +++ b/common/status_monitor/package.xml @@ -2,7 +2,7 @@ status_monitor - 5.4.0 + 6.0.1 none Tatsuya Yamasaki Apache License 2.0 diff --git a/external/concealer/CHANGELOG.rst b/external/concealer/CHANGELOG.rst index 56f25c34b5c..319ace2b4e1 100644 --- a/external/concealer/CHANGELOG.rst +++ b/external/concealer/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package concealer * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/external/concealer/package.xml b/external/concealer/package.xml index e9149818016..f22b6266719 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -2,7 +2,7 @@ concealer - 5.4.0 + 6.0.1 Provides a class 'Autoware' to conceal miscellaneous things to simplify Autoware management of the simulator. Tatsuya Yamasaki Apache License 2.0 diff --git a/external/embree_vendor/CHANGELOG.rst b/external/embree_vendor/CHANGELOG.rst index e078e79ecbb..0aef4f48d2f 100644 --- a/external/embree_vendor/CHANGELOG.rst +++ b/external/embree_vendor/CHANGELOG.rst @@ -24,6 +24,18 @@ Changelog for package embree_vendor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/external/embree_vendor/package.xml b/external/embree_vendor/package.xml index a6c6e504c1e..ec460ee3e5e 100644 --- a/external/embree_vendor/package.xml +++ b/external/embree_vendor/package.xml @@ -2,7 +2,7 @@ embree_vendor - 5.4.0 + 6.0.1 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 03ec249411c..1c0490c6432 100644 --- a/map/kashiwanoha_map/CHANGELOG.rst +++ b/map/kashiwanoha_map/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package kashiwanoha_map * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/map/kashiwanoha_map/package.xml b/map/kashiwanoha_map/package.xml index 753dd7ac815..2a2637c7f03 100644 --- a/map/kashiwanoha_map/package.xml +++ b/map/kashiwanoha_map/package.xml @@ -2,7 +2,7 @@ kashiwanoha_map - 5.4.0 + 6.0.1 map package for kashiwanoha Masaya Kataoka Apache License 2.0 diff --git a/map/simple_cross_map/CHANGELOG.rst b/map/simple_cross_map/CHANGELOG.rst index d6d95218215..0211949e15f 100644 --- a/map/simple_cross_map/CHANGELOG.rst +++ b/map/simple_cross_map/CHANGELOG.rst @@ -9,6 +9,18 @@ Changelog for package simple_cross_map * Merge branch 'master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/map/simple_cross_map/package.xml b/map/simple_cross_map/package.xml index dbcb98a773e..4a29dd5948d 100644 --- a/map/simple_cross_map/package.xml +++ b/map/simple_cross_map/package.xml @@ -2,7 +2,7 @@ simple_cross_map - 5.4.0 + 6.0.1 map package for simple cross Masaya Kataoka Apache License 2.0 diff --git a/mock/cpp_mock_scenarios/CHANGELOG.rst b/mock/cpp_mock_scenarios/CHANGELOG.rst index 6d41e0d8a32..056761fbe6a 100644 --- a/mock/cpp_mock_scenarios/CHANGELOG.rst +++ b/mock/cpp_mock_scenarios/CHANGELOG.rst @@ -21,6 +21,20 @@ Changelog for package cpp_mock_scenarios * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge pull request `#1458 `_ from tier4/refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* chore: fix build error +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/mock/cpp_mock_scenarios/package.xml b/mock/cpp_mock_scenarios/package.xml index 50634e36c70..b7bfaab3781 100644 --- a/mock/cpp_mock_scenarios/package.xml +++ b/mock/cpp_mock_scenarios/package.xml @@ -2,7 +2,7 @@ cpp_mock_scenarios - 5.4.0 + 6.0.1 C++ mock scenarios masaya Apache License 2.0 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 9b6c59895ee..eed467535ce 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 @@ -50,7 +50,8 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar to_entity && to_entity->laneMatchingSucceed()) { return traffic_simulator::distance::lateralDistance( from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), false, api_.getHdmapUtils()); + to_entity->getCanonicalizedLaneletPose().value(), + traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); } } return std::nullopt; @@ -66,8 +67,8 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar auto to_entity_lanelet_pose = to_entity->getCanonicalizedLaneletPose(matching_distance); if (from_entity_lanelet_pose && to_entity_lanelet_pose) { return traffic_simulator::distance::lateralDistance( - from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(), false, - api_.getHdmapUtils()); + from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(), + traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); } } } @@ -84,8 +85,8 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar to_entity && to_entity->laneMatchingSucceed()) { return traffic_simulator::distance::longitudinalDistance( from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), false, true, false, - api_.getHdmapUtils()); + to_entity->getCanonicalizedLaneletPose().value(), false, true, + traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils()); } } return std::nullopt; diff --git a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst index 2d0e44141d1..adf5ab3f594 100644 --- a/openscenario/openscenario_experimental_catalog/CHANGELOG.rst +++ b/openscenario/openscenario_experimental_catalog/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_experimental_catalog * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_experimental_catalog/package.xml b/openscenario/openscenario_experimental_catalog/package.xml index cfa84ab2af8..6ed24b2f812 100644 --- a/openscenario/openscenario_experimental_catalog/package.xml +++ b/openscenario/openscenario_experimental_catalog/package.xml @@ -2,7 +2,7 @@ openscenario_experimental_catalog - 5.4.0 + 6.0.1 TIER IV experimental catalogs for OpenSCENARIO Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter/CHANGELOG.rst b/openscenario/openscenario_interpreter/CHANGELOG.rst index 41b0377ec14..f73e990384a 100644 --- a/openscenario/openscenario_interpreter/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter/CHANGELOG.rst @@ -32,6 +32,20 @@ Changelog for package openscenario_interpreter * add publish_empty_context parameter * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge pull request `#1458 `_ from tier4/refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* chore: fix build error +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp index 393b4a11420..1891391ca86 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp @@ -176,9 +176,11 @@ class SimulatorCore const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose { - const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest); + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = + (routing_algorithm == RoutingAlgorithm::value_type::shortest); return traffic_simulator::pose::relativeLaneletPose( - from_lanelet_pose, to_lanelet_pose, allow_lane_change, core->getHdmapUtils()); + from_lanelet_pose, to_lanelet_pose, routing_configuration, core->getHdmapUtils()); } static auto makeNativeBoundingBoxRelativeLanePosition( @@ -221,10 +223,12 @@ class SimulatorCore const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> traffic_simulator::LaneletPose { - const bool allow_lane_change = (routing_algorithm == RoutingAlgorithm::value_type::shortest); + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = + (routing_algorithm == RoutingAlgorithm::value_type::shortest); return traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from_lanelet_pose, from_bounding_box, to_lanelet_pose, to_bounding_box, allow_lane_change, - core->getHdmapUtils()); + from_lanelet_pose, from_bounding_box, to_lanelet_pose, to_bounding_box, + routing_configuration, core->getHdmapUtils()); } static auto makeNativeBoundingBoxRelativeWorldPosition( @@ -261,14 +265,15 @@ class SimulatorCore const std::string & from_entity_name, const std::string & to_entity_name, const RoutingAlgorithm::value_type routing_algorithm = RoutingAlgorithm::undefined) -> int { + traffic_simulator::RoutingConfiguration routing_configuration; + routing_configuration.allow_lane_change = + (routing_algorithm == RoutingAlgorithm::value_type::shortest); if (const auto from_entity = core->getEntity(from_entity_name)) { if (const auto to_entity = core->getEntity(to_entity_name)) { - const bool allow_lane_change = - (routing_algorithm == RoutingAlgorithm::value_type::shortest); if ( auto lane_changes = traffic_simulator::distance::countLaneChanges( from_entity->getCanonicalizedLaneletPose().value(), - to_entity->getCanonicalizedLaneletPose().value(), allow_lane_change, + to_entity->getCanonicalizedLaneletPose().value(), routing_configuration, core->getHdmapUtils())) { return lane_changes.value().first - lane_changes.value().second; } diff --git a/openscenario/openscenario_interpreter/package.xml b/openscenario/openscenario_interpreter/package.xml index 67061214085..0594129d5c4 100644 --- a/openscenario/openscenario_interpreter/package.xml +++ b/openscenario/openscenario_interpreter/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter - 5.4.0 + 6.0.1 OpenSCENARIO 1.2.0 interpreter package for Autoware Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_example/CHANGELOG.rst b/openscenario/openscenario_interpreter_example/CHANGELOG.rst index fcb5371a752..13c22e8ca5c 100644 --- a/openscenario/openscenario_interpreter_example/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_example/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_interpreter_example * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index a649811f2cd..4c50536c972 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -3,7 +3,7 @@ openscenario_interpreter_example - 5.4.0 + 6.0.1 Examples for some TIER IV OpenSCENARIO Interpreter's features Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst index ccb02dfcb53..8cddc31ca7e 100644 --- a/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_interpreter_msgs/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_interpreter_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_interpreter_msgs/package.xml b/openscenario/openscenario_interpreter_msgs/package.xml index 56504a4bf65..91416b44bdb 100644 --- a/openscenario/openscenario_interpreter_msgs/package.xml +++ b/openscenario/openscenario_interpreter_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_interpreter_msgs - 5.4.0 + 6.0.1 ROS message types for package openscenario_interpreter Yamasaki Tatsuya Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor/CHANGELOG.rst b/openscenario/openscenario_preprocessor/CHANGELOG.rst index 332576d0113..2d9aeb97a40 100644 --- a/openscenario/openscenario_preprocessor/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_preprocessor * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_preprocessor/package.xml b/openscenario/openscenario_preprocessor/package.xml index b96a408d60c..0d78dfe8b0f 100644 --- a/openscenario/openscenario_preprocessor/package.xml +++ b/openscenario/openscenario_preprocessor/package.xml @@ -3,7 +3,7 @@ openscenario_preprocessor - 5.4.0 + 6.0.1 Example package for TIER IV OpenSCENARIO Interpreter Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst index 987bbf95c79..e4e9268cd32 100644 --- a/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst +++ b/openscenario/openscenario_preprocessor_msgs/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_preprocessor_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_preprocessor_msgs/package.xml b/openscenario/openscenario_preprocessor_msgs/package.xml index be7f0b059a3..84f04b97f11 100644 --- a/openscenario/openscenario_preprocessor_msgs/package.xml +++ b/openscenario/openscenario_preprocessor_msgs/package.xml @@ -2,7 +2,7 @@ openscenario_preprocessor_msgs - 5.4.0 + 6.0.1 ROS message types for package openscenario_preprocessor Kotaro Yoshimoto Apache License 2.0 diff --git a/openscenario/openscenario_utility/CHANGELOG.rst b/openscenario/openscenario_utility/CHANGELOG.rst index 89edaa94fa0..e332044f194 100644 --- a/openscenario/openscenario_utility/CHANGELOG.rst +++ b/openscenario/openscenario_utility/CHANGELOG.rst @@ -24,6 +24,18 @@ Changelog for package openscenario_utility * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_utility/package.xml b/openscenario/openscenario_utility/package.xml index 9904eb3042c..31705debe91 100644 --- a/openscenario/openscenario_utility/package.xml +++ b/openscenario/openscenario_utility/package.xml @@ -2,7 +2,7 @@ openscenario_utility - 5.4.0 + 6.0.1 Utility tools for ASAM OpenSCENARIO 1.2.0 Tatsuya Yamasaki Apache License 2.0 diff --git a/openscenario/openscenario_validator/CHANGELOG.rst b/openscenario/openscenario_validator/CHANGELOG.rst index e9c9fe29433..9d6cd0b3cba 100644 --- a/openscenario/openscenario_validator/CHANGELOG.rst +++ b/openscenario/openscenario_validator/CHANGELOG.rst @@ -10,6 +10,18 @@ Changelog for package openscenario_validator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/openscenario/openscenario_validator/package.xml b/openscenario/openscenario_validator/package.xml index fff82a063f1..475e1c2449a 100644 --- a/openscenario/openscenario_validator/package.xml +++ b/openscenario/openscenario_validator/package.xml @@ -2,7 +2,7 @@ openscenario_validator - 5.4.0 + 6.0.1 Validator for OpenSCENARIO 1.3 Kotaro Yoshimoto Apache License 2.0 diff --git a/rviz_plugins/openscenario_visualization/CHANGELOG.rst b/rviz_plugins/openscenario_visualization/CHANGELOG.rst index aab3e082850..441c32d2d60 100644 --- a/rviz_plugins/openscenario_visualization/CHANGELOG.rst +++ b/rviz_plugins/openscenario_visualization/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_visualization * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/rviz_plugins/openscenario_visualization/package.xml b/rviz_plugins/openscenario_visualization/package.xml index ebd61d649ca..88a2d554a50 100644 --- a/rviz_plugins/openscenario_visualization/package.xml +++ b/rviz_plugins/openscenario_visualization/package.xml @@ -2,7 +2,7 @@ openscenario_visualization - 5.4.0 + 6.0.1 Visualization tools for simulation results Masaya Kataoka Kyoichi Sugahara diff --git a/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst b/rviz_plugins/real_time_factor_control_rviz_plugin/CHANGELOG.rst index e96ead69921..c7a7cd944fd 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,18 @@ Changelog for package real_time_factor_control_rviz_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph 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 069195fd8c2..b07422dfb1b 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 - 5.4.0 + 6.0.1 Slider controlling real time factor value. Paweł Lech Apache License 2.0 diff --git a/scenario_simulator_v2/CHANGELOG.rst b/scenario_simulator_v2/CHANGELOG.rst index 59f1c4d82c3..091d6fc2755 100644 --- a/scenario_simulator_v2/CHANGELOG.rst +++ b/scenario_simulator_v2/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package scenario_simulator_v2 * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/scenario_simulator_v2/package.xml b/scenario_simulator_v2/package.xml index 9b1413f997c..3d7a55a754c 100644 --- a/scenario_simulator_v2/package.xml +++ b/scenario_simulator_v2/package.xml @@ -2,7 +2,7 @@ scenario_simulator_v2 - 5.4.0 + 6.0.1 scenario testing framework for Autoware Masaya Kataoka Apache License 2.0 diff --git a/simulation/behavior_tree_plugin/CHANGELOG.rst b/simulation/behavior_tree_plugin/CHANGELOG.rst index 64292dfee99..3e4f1ab053a 100644 --- a/simulation/behavior_tree_plugin/CHANGELOG.rst +++ b/simulation/behavior_tree_plugin/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package behavior_tree_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/simulation/behavior_tree_plugin/package.xml b/simulation/behavior_tree_plugin/package.xml index 428b0f319eb..9f0cea345b0 100644 --- a/simulation/behavior_tree_plugin/package.xml +++ b/simulation/behavior_tree_plugin/package.xml @@ -2,7 +2,7 @@ behavior_tree_plugin - 5.4.0 + 6.0.1 Behavior tree plugin for traffic_simulator masaya Apache 2.0 diff --git a/simulation/do_nothing_plugin/CHANGELOG.rst b/simulation/do_nothing_plugin/CHANGELOG.rst index a069c10af4a..37024db7ae1 100644 --- a/simulation/do_nothing_plugin/CHANGELOG.rst +++ b/simulation/do_nothing_plugin/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package do_nothing_plugin * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/simulation/do_nothing_plugin/package.xml b/simulation/do_nothing_plugin/package.xml index fb8e7ba9ad9..6d166ffd3ec 100644 --- a/simulation/do_nothing_plugin/package.xml +++ b/simulation/do_nothing_plugin/package.xml @@ -2,7 +2,7 @@ do_nothing_plugin - 5.4.0 + 6.0.1 Behavior plugin for do nothing Masaya Kataoka Apache 2.0 diff --git a/simulation/simple_sensor_simulator/CHANGELOG.rst b/simulation/simple_sensor_simulator/CHANGELOG.rst index 4a8a02e69d9..2e5f73ae879 100644 --- a/simulation/simple_sensor_simulator/CHANGELOG.rst +++ b/simulation/simple_sensor_simulator/CHANGELOG.rst @@ -21,6 +21,27 @@ Changelog for package simple_sensor_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ +* Merge pull request `#1442 `_ from tier4/feat/add-glog-simple-sensor-simulator + Feat/add glog simple sensor simulator +* fix: update cmake +* fix: implementation +* Revert "fix: use glog_vendor" + This reverts commit 96f6c540fad3faa43c6a8eb82c48c467e746675f. +* fix: use glog_vendor +* feat(simple_sensor_simulator): add glog +* Contributors: Masaya Kataoka, satoshi-ota + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/simulation/simple_sensor_simulator/CMakeLists.txt b/simulation/simple_sensor_simulator/CMakeLists.txt index ae8e1794af2..a86a71e2de1 100644 --- a/simulation/simple_sensor_simulator/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/CMakeLists.txt @@ -56,6 +56,7 @@ ament_auto_add_library(simple_sensor_simulator_component SHARED src/vehicle_simulation/vehicle_model/sim_model_interface.cpp ) target_link_libraries(simple_sensor_simulator_component + glog pthread sodium zmq diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index 0c46bbb97ee..0d51342883f 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -1,7 +1,7 @@ simple_sensor_simulator - 5.4.0 + 6.0.1 simple_sensor_simulator package masaya kataoka @@ -40,6 +40,7 @@ boost eigen embree_vendor + libgoogle-glog-dev libpcl-all-dev nav_msgs pcl_conversions diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator_node.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator_node.cpp index 5218378d877..7f8133fffb6 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator_node.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator_node.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include #include @@ -19,6 +21,9 @@ int main(int argc, char * argv[]) { + google::InitGoogleLogging(argv[0]); + google::InstallFailureSignalHandler(); + rclcpp::init(argc, argv); rclcpp::NodeOptions options; auto component = std::make_shared(options); diff --git a/simulation/simulation_interface/CHANGELOG.rst b/simulation/simulation_interface/CHANGELOG.rst index d44fe07cef0..3c6fdb2b4ef 100644 --- a/simulation/simulation_interface/CHANGELOG.rst +++ b/simulation/simulation_interface/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package simulation_interface * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index e40749ad095..5fe2774e7c7 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -2,7 +2,7 @@ simulation_interface - 5.4.0 + 6.0.1 packages to define interface between simulator and scenario interpreter Masaya Kataoka Apache License 2.0 diff --git a/simulation/traffic_simulator/CHANGELOG.rst b/simulation/traffic_simulator/CHANGELOG.rst index eb7f6f8563a..bf2efc7032c 100644 --- a/simulation/traffic_simulator/CHANGELOG.rst +++ b/simulation/traffic_simulator/CHANGELOG.rst @@ -21,6 +21,42 @@ Changelog for package traffic_simulator * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge pull request `#1458 `_ from tier4/refactor/add_routing_graph_argument +* docs: add comment for HdMapUtils::DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO +* refactor: delete unused hash function +* Merge branch 'master' into refactor/add_routing_graph_argument +* chore: fix spell miss +* chore: fix build error +* chore: fix linelint error +* feat: introduce RoutingConfiguration to traffic_simulator::pose +* feat: introduce RoutingConfiguration to traffic_simulator::distance +* feat: introduce RoutingConfiguration to CanonicalizedLaneletPose +* feat: introduce RoutingConfiguration to HDMapUtils +* feat: add RoutingConfiguration +* refactor: add routing graph argument to HdMapUtils::getFollowingLanelets +* refactor: add routing graph argument to HdMapUtils::getAlongLaneletPose +* refactor: add routing graph argument to HdMapUtils::getLongitudinalDistance +* refactor: add routing graph argument to HdMapUtils::getLateralDistance +* fix: correct wrong usage of toLaneletPose +* Merge branch 'master' into refactor/add_routing_graph_argument +* refactor: add routing graph argument to HdMapUtils::getLaneChangeableLaneletId +* refactor: add routing graph argument to HdMapUtils::getSpeedLimit +* refactor: add routing graph argument to HdMapUtils::toLaneletPose(s) +* refactor: add routing graph argument to HdMapUtils::matchToLane +* refactor: add routing graph argument to HdMapUtils::getConflictingLaneIds +* refactor: add routing graph argument to HdMapUtils::countLaneChanges +* refactor: add routing graph argument to HdMapUtils::canChangeLane +* refactor: add routing graph argument to HdMapUtils::getPreviousLanelets +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge pull request `#1456 `_ from tier4/feature/shoulder_routing_graph 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 a1d9d83b7bf..34d75b4e1f3 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 @@ -41,7 +41,8 @@ class CanonicalizedLaneletPose auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - bool allow_lane_change = false) const -> std::optional; + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional; static auto setConsiderPoseByRoadSlope(bool consider_pose_by_road_slope) -> void { consider_pose_by_road_slope_ = consider_pose_by_road_slope; diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp new file mode 100644 index 00000000000..b67e630a4d7 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp @@ -0,0 +1,44 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__DATA_TYPE__ROUTING_CONFIGURATIONS_HPP_ +#define TRAFFIC_SIMULATOR__DATA_TYPE__ROUTING_CONFIGURATIONS_HPP_ + +#include +#include + +namespace traffic_simulator +{ +struct RoutingConfiguration +{ + bool allow_lane_change = false; + traffic_simulator::RoutingGraphType routing_graph_type = + traffic_simulator::RoutingGraphType::VEHICLE; + + bool operator==(const RoutingConfiguration & routing_configuration) const + { + return allow_lane_change == routing_configuration.allow_lane_change && + routing_graph_type == routing_configuration.routing_graph_type; + } + + friend std::ostream & operator<<(std::ostream & os, const RoutingConfiguration & rc) + { + os << "{ allow_lane_change: " << (rc.allow_lane_change ? "true" : "false") + << ", routing_graph_type: " << to_string(rc.routing_graph_type) << " }"; + return os; + } +}; +} // namespace traffic_simulator + +#endif // TRAFFIC_SIMULATOR__DATA_TYPE__ROUTING_CONFIGURATIONS_HPP_ 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 78c975bbb44..9e6de237138 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 @@ -45,6 +45,7 @@ #include #include #include +#include #include #include #include @@ -65,7 +66,10 @@ class HdMapUtils public: explicit HdMapUtils(const boost::filesystem::path &, const geographic_msgs::msg::GeoPoint &); - auto canChangeLane(const lanelet::Id from, const lanelet::Id to) const -> bool; + auto canChangeLane( + const lanelet::Id from, const lanelet::Id to, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> bool; auto canonicalizeLaneletPose(const traffic_simulator_msgs::msg::LaneletPose &) const -> std::tuple< @@ -82,8 +86,9 @@ class HdMapUtils auto countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const - -> std::optional>; + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional>; auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids; @@ -93,8 +98,9 @@ class HdMapUtils -> std::vector; auto getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from, const double along) const - -> traffic_simulator_msgs::msg::LaneletPose; + const traffic_simulator_msgs::msg::LaneletPose & from, const double along, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + const -> traffic_simulator_msgs::msg::LaneletPose; auto getCenterPoints(const lanelet::Ids &) const -> std::vector; @@ -113,7 +119,9 @@ class HdMapUtils auto getConflictingCrosswalkIds(const lanelet::Ids &) const -> lanelet::Ids; - auto getConflictingLaneIds(const lanelet::Ids &) const -> lanelet::Ids; + auto getConflictingLaneIds( + const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getDistanceToStopLine( const lanelet::Ids & route_lanelets, @@ -141,11 +149,14 @@ class HdMapUtils auto getFollowingLanelets( const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon = 100, - const bool include_current_lanelet_id = true) const -> lanelet::Ids; + const bool include_current_lanelet_id = true, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getFollowingLanelets( - const lanelet::Id, const double distance = 100, const bool include_self = true) const - -> lanelet::Ids; + const lanelet::Id, const double distance = 100, const bool include_self = true, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double; @@ -162,12 +173,14 @@ class HdMapUtils -> std::optional>; auto getLaneChangeableLaneletId( - const lanelet::Id, const traffic_simulator::lane_change::Direction) const - -> std::optional; + const lanelet::Id, const traffic_simulator::lane_change::Direction, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional; auto getLaneChangeableLaneletId( - const lanelet::Id, const traffic_simulator::lane_change::Direction, - const std::uint8_t shift) const -> std::optional; + const lanelet::Id, const traffic_simulator::lane_change::Direction, const std::uint8_t shift, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional; auto getLaneletIds() const -> lanelet::Ids; @@ -179,8 +192,9 @@ class HdMapUtils auto getLateralDistance( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change = false) const - -> std::optional; + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional; auto getLeftBound(const lanelet::Id) const -> std::vector; @@ -191,7 +205,8 @@ class HdMapUtils auto getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, - const bool allow_lane_change = false) const -> std::optional; + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> std::optional; auto getNearbyLaneletIds( const geometry_msgs::msg::Point &, const double distance_threshold, @@ -241,8 +256,10 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; - auto getPreviousLanelets(const lanelet::Id, const double backward_horizon = 100) const - -> lanelet::Ids; + auto getPreviousLanelets( + const lanelet::Id, const double backward_horizon = 100, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; auto getRightBound(const lanelet::Id) const -> std::vector; @@ -256,11 +273,13 @@ class HdMapUtils auto getRightOfWayLaneletIds(const lanelet::Id) const -> lanelet::Ids; auto getRoute( - const lanelet::Id from, const lanelet::Id to, bool allow_lane_change = false, - const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; + const lanelet::Id from, const lanelet::Id to, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) const -> lanelet::Ids; - auto getSpeedLimit(const lanelet::Ids &) const -> double; + auto getSpeedLimit( + const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> double; auto getStopLineIds() const -> lanelet::Ids; @@ -299,10 +318,18 @@ class HdMapUtils auto isTrafficLightRegulatoryElement(const lanelet::Id) const -> bool; +private: + /// @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; + +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 = 0.8) const -> std::optional; + const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO, + const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE) const -> std::optional; auto toLaneletPose( const geometry_msgs::msg::Pose &, const bool include_crosswalk, @@ -316,13 +343,15 @@ class HdMapUtils auto toLaneletPose( const geometry_msgs::msg::Point &, const traffic_simulator_msgs::msg::BoundingBox &, - const bool include_crosswalk, const double matching_distance = 1.0) const - -> std::optional; + const bool include_crosswalk, const double matching_distance = 1.0, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + 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 - -> std::optional; + const bool include_crosswalk, const double matching_distance = 1.0, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + const -> std::optional; auto toLaneletPose( const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 1.0) const @@ -330,8 +359,9 @@ class HdMapUtils auto toLaneletPoses( const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 5.0, - const bool include_opposite_direction = true) const - -> std::vector; + const bool include_opposite_direction = true, + const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) + const -> std::vector; auto toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin; @@ -372,8 +402,9 @@ class HdMapUtils [[nodiscard]] auto getRoute( const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, - lanelet::LaneletMapPtr lanelet_map_ptr, const bool allow_lane_change, - const traffic_simulator::RoutingGraphType type) -> lanelet::Ids; + lanelet::LaneletMapPtr lanelet_map_ptr, + const traffic_simulator::RoutingConfiguration & routing_configuration = + traffic_simulator::RoutingConfiguration()) -> lanelet::Ids; private: [[nodiscard]] RouteCache & route_cache(const traffic_simulator::RoutingGraphType type); diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp index 80b1a287e88..7919a140491 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp @@ -25,24 +25,26 @@ inline namespace distance // Lateral auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional; + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional; auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - double matching_distance, bool allow_lane_change, + double matching_distance, const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; // Lateral (unit: lanes) auto countLaneChanges( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional>; // Longitudinal auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool include_adjacent_lanelet, bool include_opposite_direction, bool allow_lane_change, + bool include_adjacent_lanelet, bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; // BoundingBox @@ -56,7 +58,8 @@ auto boundingBoxLaneLateralDistance( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; auto boundingBoxLaneLongitudinalDistance( @@ -64,7 +67,8 @@ auto boundingBoxLaneLongitudinalDistance( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet, - bool include_opposite_direction, bool allow_lane_change, + bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional; // Bounds diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index 37428420fde..9dc3b8053f8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -80,14 +80,15 @@ auto boundingBoxRelativePose( // Relative LaneletPose auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> LaneletPose; + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; 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 bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose; // Others diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index fb852f1ac83..9c154ad3dbe 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -1,7 +1,7 @@ traffic_simulator - 5.4.0 + 6.0.1 control traffic flow masaya kataoka diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index d9b37d19eb7..b4dfb4dc0d8 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -111,17 +111,18 @@ auto CanonicalizedLaneletPose::canonicalize( auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( LaneletPose from, const std::shared_ptr & hdmap_utils, - bool allow_lane_change) const -> std::optional + const traffic_simulator::RoutingConfiguration & routing_configuration) const + -> std::optional { if (lanelet_poses_.empty()) { return std::nullopt; } lanelet::Ids shortest_route = - hdmap_utils->getRoute(from.lanelet_id, lanelet_poses_[0].lanelet_id); + hdmap_utils->getRoute(from.lanelet_id, lanelet_poses_[0].lanelet_id, routing_configuration); LaneletPose alternative_lanelet_pose = lanelet_poses_[0]; for (const auto & laneletPose : lanelet_poses_) { const auto route = - hdmap_utils->getRoute(from.lanelet_id, laneletPose.lanelet_id, allow_lane_change); + hdmap_utils->getRoute(from.lanelet_id, laneletPose.lanelet_id, routing_configuration); if (shortest_route.size() > route.size()) { shortest_route = route; alternative_lanelet_pose = laneletPose; diff --git a/simulation/traffic_simulator/src/entity/entity_base.cpp b/simulation/traffic_simulator/src/entity/entity_base.cpp index 5537f99c48e..4a283710e8d 100644 --- a/simulation/traffic_simulator/src/entity/entity_base.cpp +++ b/simulation/traffic_simulator/src/entity/entity_base.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -714,8 +715,12 @@ auto EntityBase::requestSynchronize( "If so please contact the developer since there might be an undiscovered bug."); } + RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto entity_distance = longitudinalDistance( - entity_lanelet_pose.value(), entity_target, true, true, true, hdmap_utils_ptr_); + entity_lanelet_pose.value(), entity_target, true, true, + lane_changeable_routing_configuration, hdmap_utils_ptr_); if (!entity_distance.has_value()) { THROW_SEMANTIC_ERROR( "Failed to get distance between entity and target lanelet pose. Check if the entity has " @@ -730,7 +735,7 @@ auto EntityBase::requestSynchronize( const auto target_entity_distance = longitudinalDistance( CanonicalizedLaneletPose(target_entity_lanelet_pose, hdmap_utils_ptr_), target_sync_pose, - true, true, true, hdmap_utils_ptr_); + true, true, 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/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 4a66bbe6eb7..e61e479cb19 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -205,10 +205,11 @@ auto HdMapUtils::canonicalizeLaneletPose( auto HdMapUtils::countLaneChanges( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional> { - const auto route = getRoute(from.lanelet_id, to.lanelet_id, allow_lane_change); + const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); if (route.empty()) { return std::nullopt; } else { @@ -217,13 +218,13 @@ auto HdMapUtils::countLaneChanges( const auto & previous = route[i - 1]; const auto & current = route[i]; - if (auto followings = getNextLaneletIds(previous); + if (auto followings = getNextLaneletIds(previous, routing_configuration.routing_graph_type); std::find(followings.begin(), followings.end(), current) == followings.end()) { - if (auto lefts = getLeftLaneletIds(previous, traffic_simulator::RoutingGraphType::VEHICLE); + if (auto lefts = getLeftLaneletIds(previous, routing_configuration.routing_graph_type); std::find(lefts.begin(), lefts.end(), current) != lefts.end()) { lane_changes.first++; } else if (auto rights = - getRightLaneletIds(previous, traffic_simulator::RoutingGraphType::VEHICLE); + getRightLaneletIds(previous, routing_configuration.routing_graph_type); std::find(rights.begin(), rights.end(), current) != rights.end()) { lane_changes.second++; } @@ -386,13 +387,15 @@ auto HdMapUtils::getCollisionPointInLaneCoordinate( return std::nullopt; } -auto HdMapUtils::getConflictingLaneIds(const lanelet::Ids & lanelet_ids) const -> lanelet::Ids +auto HdMapUtils::getConflictingLaneIds( + const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { lanelet::Ids ids; for (const auto & lanelet_id : lanelet_ids) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - const auto conflicting_lanelets = lanelet::utils::getConflictingLanelets( - routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE), lanelet); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graphs_->routing_graph(type), lanelet); for (const auto & conflicting_lanelet : conflicting_lanelets) { ids.emplace_back(conflicting_lanelet.id()); } @@ -521,8 +524,8 @@ auto HdMapUtils::toPoint2d(const geometry_msgs::msg::Point & point) const -> lan auto HdMapUtils::matchToLane( const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox, - const bool include_crosswalk, const double matching_distance, const double reduction_ratio) const - -> std::optional + 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); @@ -542,7 +545,7 @@ auto HdMapUtils::matchToLane( lanelet::matching::getDeterministicMatches(*lanelet_map_ptr_, obj, matching_distance); if (!include_crosswalk) { matches = lanelet::matching::removeNonRuleCompliantMatches( - matches, routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE)); + matches, routing_graphs_->traffic_rule(type)); } if (matches.empty()) { return std::nullopt; @@ -626,21 +629,24 @@ auto HdMapUtils::toLaneletPose( 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 + 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); + 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 + 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); + 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); } @@ -648,36 +654,35 @@ auto HdMapUtils::toLaneletPose( if (pose_in_target_lanelet) { return pose_in_target_lanelet; } - const auto previous = getPreviousLaneletIds(lanelet_id.value()); + 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()); + 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); + 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 + 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, traffic_simulator::RoutingGraphType::VEHICLE, include_opposite_direction); - lanelet_ids += getRightLaneletIds( - lanelet_id, traffic_simulator::RoutingGraphType::VEHICLE, include_opposite_direction); - lanelet_ids += getPreviousLaneletIds(lanelet_ids); - lanelet_ids += getNextLaneletIds(lanelet_ids); + 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()); @@ -718,7 +723,8 @@ auto HdMapUtils::getClosestLaneletId( } } -auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double +auto HdMapUtils::getSpeedLimit( + const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const -> double { std::vector limits; if (lanelet_ids.empty()) { @@ -726,8 +732,7 @@ auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double } for (auto itr = lanelet_ids.begin(); itr != lanelet_ids.end(); itr++) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(*itr); - const auto limit = routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE) - ->speedLimit(lanelet); + const auto limit = routing_graphs_->traffic_rule(type)->speedLimit(lanelet); limits.push_back(lanelet::units::KmHQuantity(limit.speedLimit).value() / 3.6); } return *std::min_element(limits.begin(), limits.end()); @@ -735,15 +740,16 @@ auto HdMapUtils::getSpeedLimit(const lanelet::Ids & lanelet_ids) const -> double auto HdMapUtils::getLaneChangeableLaneletId( const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction, - const std::uint8_t shift) const -> std::optional + const std::uint8_t shift, const traffic_simulator::RoutingGraphType type) const + -> std::optional { if (shift == 0) { return getLaneChangeableLaneletId( - lanelet_id, traffic_simulator::lane_change::Direction::STRAIGHT); + lanelet_id, traffic_simulator::lane_change::Direction::STRAIGHT, type); } else { auto reference_id = lanelet_id; for (uint8_t i = 0; i < shift; i++) { - auto id = getLaneChangeableLaneletId(reference_id, direction); + auto id = getLaneChangeableLaneletId(reference_id, direction, type); if (!id) { return std::nullopt; } else { @@ -758,8 +764,8 @@ auto HdMapUtils::getLaneChangeableLaneletId( } auto HdMapUtils::getLaneChangeableLaneletId( - const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction) const - -> std::optional + const lanelet::Id lanelet_id, const traffic_simulator::lane_change::Direction direction, + const traffic_simulator::RoutingGraphType type) const -> std::optional { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); std::optional target = std::nullopt; @@ -768,19 +774,13 @@ auto HdMapUtils::getLaneChangeableLaneletId( target = lanelet.id(); break; case traffic_simulator::lane_change::Direction::LEFT: - if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->left(lanelet)) { - target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->left(lanelet) - ->id(); + if (routing_graphs_->routing_graph(type)->left(lanelet)) { + target = routing_graphs_->routing_graph(type)->left(lanelet)->id(); } break; case traffic_simulator::lane_change::Direction::RIGHT: - if (routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->right(lanelet)) { - target = routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->right(lanelet) - ->id(); + if (routing_graphs_->routing_graph(type)->right(lanelet)) { + target = routing_graphs_->routing_graph(type)->right(lanelet)->id(); } break; } @@ -788,18 +788,20 @@ auto HdMapUtils::getLaneChangeableLaneletId( } auto HdMapUtils::getPreviousLanelets( - const lanelet::Id current_lanelet_id, const double backward_horizon) const -> lanelet::Ids + const lanelet::Id current_lanelet_id, const double backward_horizon, + const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids { lanelet::Ids previous_lanelets_ids; double total_distance = 0.0; previous_lanelets_ids.push_back(current_lanelet_id); 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"); + if (const auto straight_lanelet_ids = + getPreviousLaneletIds(reference_lanelet_id, "straight", type); not straight_lanelet_ids.empty()) { total_distance = total_distance + getLaneletLength(straight_lanelet_ids[0]); previous_lanelets_ids.push_back(straight_lanelet_ids[0]); - } else if (auto non_straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id); + } else if (auto non_straight_lanelet_ids = getPreviousLaneletIds(reference_lanelet_id, type); not non_straight_lanelet_ids.empty()) { total_distance = total_distance + getLaneletLength(non_straight_lanelet_ids[0]); previous_lanelets_ids.push_back(non_straight_lanelet_ids[0]); @@ -817,11 +819,12 @@ auto HdMapUtils::isInRoute(const lanelet::Id lanelet_id, const lanelet::Ids & ro auto HdMapUtils::getFollowingLanelets( const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon, - const bool include_current_lanelet_id) const -> lanelet::Ids + const bool include_current_lanelet_id, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { const auto is_following_lanelet = - [this](const auto & current_lanelet, const auto & candidate_lanelet) { - const auto next_ids = getNextLaneletIds(current_lanelet); + [this, type](const auto & current_lanelet, const auto & candidate_lanelet) { + const auto next_ids = getNextLaneletIds(current_lanelet, type); return std::find(next_ids.cbegin(), next_ids.cend(), candidate_lanelet) != next_ids.cend(); }; @@ -858,7 +861,7 @@ auto HdMapUtils::getFollowingLanelets( THROW_SEMANTIC_ERROR("lanelet id does not match"); } else if (total_distance <= horizon) { const auto remaining_lanelets = - getFollowingLanelets(route.back(), horizon - total_distance, false); + getFollowingLanelets(route.back(), horizon - total_distance, false, type); following_lanelets_ids.insert( following_lanelets_ids.end(), remaining_lanelets.begin(), remaining_lanelets.end()); } @@ -867,8 +870,8 @@ auto HdMapUtils::getFollowingLanelets( } auto HdMapUtils::getFollowingLanelets( - const lanelet::Id lanelet_id, const double distance, const bool include_self) const - -> lanelet::Ids + const lanelet::Id lanelet_id, const double distance, const bool include_self, + const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids { lanelet::Ids ret; double total_distance = 0.0; @@ -877,13 +880,13 @@ auto HdMapUtils::getFollowingLanelets( } lanelet::Id end_lanelet_id = lanelet_id; while (total_distance < distance) { - if (const auto straight_ids = getNextLaneletIds(end_lanelet_id, "straight"); + if (const auto straight_ids = getNextLaneletIds(end_lanelet_id, "straight", type); !straight_ids.empty()) { total_distance = total_distance + getLaneletLength(straight_ids[0]); ret.push_back(straight_ids[0]); end_lanelet_id = straight_ids[0]; continue; - } else if (const auto ids = getNextLaneletIds(end_lanelet_id); ids.size() != 0) { + } else if (const auto ids = getNextLaneletIds(end_lanelet_id, type); ids.size() != 0) { total_distance = total_distance + getLaneletLength(ids[0]); ret.push_back(ids[0]); end_lanelet_id = ids[0]; @@ -896,11 +899,11 @@ auto HdMapUtils::getFollowingLanelets( } auto HdMapUtils::getRoute( - const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, bool allow_lane_change, - const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const traffic_simulator::RoutingConfiguration & routing_configuration) const -> lanelet::Ids { return routing_graphs_->getRoute( - from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, allow_lane_change, type); + from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, routing_configuration); } auto HdMapUtils::getCenterPointsSpline(const lanelet::Id lanelet_id) const @@ -1125,16 +1128,16 @@ auto HdMapUtils::getTrafficLightBulbPosition( } auto HdMapUtils::getAlongLaneletPose( - const traffic_simulator_msgs::msg::LaneletPose & from_pose, const double along) const - -> traffic_simulator_msgs::msg::LaneletPose + 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"); + auto next_ids = getNextLaneletIds(along_pose.lanelet_id, "straight", type); if (next_ids.empty()) { - next_ids = getNextLaneletIds(along_pose.lanelet_id); + 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, ",", @@ -1146,9 +1149,9 @@ auto HdMapUtils::getAlongLaneletPose( } } else { while (along_pose.s < 0) { - auto previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, "straight"); + auto previous_ids = getPreviousLaneletIds(along_pose.lanelet_id, "straight", type); if (previous_ids.empty()) { - previous_ids = getPreviousLaneletIds(along_pose.lanelet_id); + 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, ",", @@ -1401,27 +1404,28 @@ auto HdMapUtils::getTangentVector(const lanelet::Id lanelet_id, const double s) } auto HdMapUtils::canChangeLane( - const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id) const -> bool + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const traffic_simulator::RoutingGraphType type) const -> bool { const auto from_lanelet = lanelet_map_ptr_->laneletLayer.get(from_lanelet_id); const auto to_lanelet = lanelet_map_ptr_->laneletLayer.get(to_lanelet_id); - return routing_graphs_->traffic_rule(traffic_simulator::RoutingGraphType::VEHICLE) - ->canChangeLane(from_lanelet, to_lanelet); + return routing_graphs_->traffic_rule(type)->canChangeLane(from_lanelet, to_lanelet); } auto HdMapUtils::getLateralDistance( const traffic_simulator_msgs::msg::LaneletPose & from, - const traffic_simulator_msgs::msg::LaneletPose & to, bool allow_lane_change) const + const traffic_simulator_msgs::msg::LaneletPose & to, + const traffic_simulator::RoutingConfiguration & routing_configuration) const -> std::optional { - const auto route = getRoute(from.lanelet_id, to.lanelet_id, allow_lane_change); + const auto route = getRoute(from.lanelet_id, to.lanelet_id, routing_configuration); if (route.empty()) { return std::nullopt; } - if (allow_lane_change) { + 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]); + auto next_lanelet_ids = getNextLaneletIds(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]; }); @@ -1457,18 +1461,21 @@ auto HdMapUtils::getLateralDistance( auto HdMapUtils::getLongitudinalDistance( const traffic_simulator_msgs::msg::LaneletPose & from_pose, const traffic_simulator_msgs::msg::LaneletPose & to_pose, - const bool allow_lane_change /*= false*/) const -> std::optional + const traffic_simulator::RoutingConfiguration & routing_configuration) const + -> std::optional { - const auto is_lane_change_required = - [this](const lanelet::Id current_lanelet, const lanelet::Id next_lanelet) -> bool { - const auto next_lanelet_ids = getNextLaneletIds(current_lanelet); + const auto is_lane_change_required = [this, 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); return std::none_of( next_lanelet_ids.cbegin(), next_lanelet_ids.cend(), [next_lanelet](const lanelet::Id id) { return id == next_lanelet; }); }; /// @sa https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/DistanceCalculation/ - if (const auto route = getRoute(from_pose.lanelet_id, to_pose.lanelet_id, allow_lane_change); + if (const auto route = getRoute(from_pose.lanelet_id, to_pose.lanelet_id, routing_configuration); not route.empty() || from_pose.lanelet_id == to_pose.lanelet_id) { /// @note horizontal bar must intersect with the lanelet spline, so the distance was set arbitrarily to 10 meters. static constexpr double matching_distance = 10.0; @@ -2222,31 +2229,32 @@ RouteCache & HdMapUtils::RoutingGraphs::route_cache(const traffic_simulator::Rou auto HdMapUtils::RoutingGraphs::getRoute( const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, - lanelet::LaneletMapPtr lanelet_map_ptr, bool allow_lane_change, - const traffic_simulator::RoutingGraphType type) -> lanelet::Ids + lanelet::LaneletMapPtr lanelet_map_ptr, + const traffic_simulator::RoutingConfiguration & routing_configuration) -> lanelet::Ids { - auto & cache = route_cache(type); - if (cache.exists(from_lanelet_id, to_lanelet_id, allow_lane_change)) { - return cache.getRoute(from_lanelet_id, to_lanelet_id, allow_lane_change); + auto & cache = route_cache(routing_configuration.routing_graph_type); + if (cache.exists(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change)) { + return cache.getRoute(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change); } lanelet::Ids ids; const auto lanelet = lanelet_map_ptr->laneletLayer.get(from_lanelet_id); const auto to_lanelet = lanelet_map_ptr->laneletLayer.get(to_lanelet_id); lanelet::Optional route = - routing_graph(type)->getRoute(lanelet, to_lanelet, 0, allow_lane_change); + routing_graph(routing_configuration.routing_graph_type) + ->getRoute(lanelet, to_lanelet, 0, routing_configuration.allow_lane_change); if (!route) { - cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); return ids; } lanelet::routing::LaneletPath shortest_path = route->shortestPath(); if (shortest_path.empty()) { - cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); return ids; } for (auto lane_itr = shortest_path.begin(); lane_itr != shortest_path.end(); lane_itr++) { ids.push_back(lane_itr->id()); } - cache.appendData(from_lanelet_id, to_lanelet_id, allow_lane_change, ids); + cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); return ids; } diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index a8d9b6175f3..98bac102b88 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -24,22 +24,22 @@ inline namespace distance { auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> std::optional + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional { return hdmap_utils_ptr->getLateralDistance( - static_cast(from), static_cast(to), allow_lane_change); + static_cast(from), static_cast(to), routing_configuration); } auto lateralDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - double matching_distance, bool allow_lane_change, + double matching_distance, const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { if ( std::abs(static_cast(from).offset) <= matching_distance && std::abs(static_cast(to).offset) <= matching_distance) { - return lateralDistance(from, to, allow_lane_change, hdmap_utils_ptr); + return lateralDistance(from, to, routing_configuration, hdmap_utils_ptr); } else { return std::nullopt; } @@ -47,17 +47,19 @@ auto lateralDistance( auto countLaneChanges( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> std::optional> { return hdmap_utils_ptr->countLaneChanges( - static_cast(from), static_cast(to), allow_lane_change); + static_cast(from), static_cast(to), routing_configuration); } /// @sa https://github.com/tier4/scenario_simulator_v2/blob/729e4e6372cdba60e377ae097d032905b80763a9/docs/developer_guide/lane_pose_calculation/GetLongitudinalDistance.md auto longitudinalDistance( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - bool include_adjacent_lanelet, bool include_opposite_direction, bool allow_lane_change, + bool include_adjacent_lanelet, bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { if (!include_adjacent_lanelet) { @@ -65,16 +67,16 @@ auto longitudinalDistance( if (to.hasAlternativeLaneletPose()) { if ( const auto to_canonicalized_opt = to.getAlternativeLaneletPoseBaseOnShortestRouteFrom( - static_cast(from), hdmap_utils_ptr, allow_lane_change)) { + static_cast(from), hdmap_utils_ptr, routing_configuration)) { to_canonicalized = to_canonicalized_opt.value(); } } const auto forward_distance = hdmap_utils_ptr->getLongitudinalDistance( - static_cast(from), to_canonicalized, allow_lane_change); + static_cast(from), to_canonicalized, routing_configuration); const auto backward_distance = hdmap_utils_ptr->getLongitudinalDistance( - to_canonicalized, static_cast(from), allow_lane_change); + to_canonicalized, static_cast(from), routing_configuration); if (forward_distance && backward_distance) { return forward_distance.value() > backward_distance.value() ? -backward_distance.value() @@ -97,12 +99,12 @@ auto longitudinalDistance( auto from_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(from), static_cast(from).lanelet_id, - matching_distance, include_opposite_direction); + matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); from_poses.emplace_back(from); auto to_poses = hdmap_utils_ptr->toLaneletPoses( static_cast(to), static_cast(to).lanelet_id, - matching_distance, include_opposite_direction); + matching_distance, include_opposite_direction, routing_configuration.routing_graph_type); to_poses.emplace_back(to); std::vector distances = {}; @@ -112,7 +114,7 @@ auto longitudinalDistance( const auto distance = longitudinalDistance( CanonicalizedLaneletPose(from_pose, hdmap_utils_ptr), CanonicalizedLaneletPose(to_pose, hdmap_utils_ptr), false, include_opposite_direction, - allow_lane_change, hdmap_utils_ptr)) { + routing_configuration, hdmap_utils_ptr)) { distances.emplace_back(distance.value()); } } @@ -141,10 +143,12 @@ auto boundingBoxLaneLateralDistance( const CanonicalizedLaneletPose & from, const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { - if (const auto lateral_distance = lateralDistance(from, to, allow_lane_change, hdmap_utils_ptr); + if (const auto lateral_distance = + lateralDistance(from, to, routing_configuration, hdmap_utils_ptr); lateral_distance) { const auto from_bounding_box_distances = math::geometry::getDistancesFromCenterToEdge(from_bounding_box); @@ -168,11 +172,12 @@ auto boundingBoxLaneLongitudinalDistance( const traffic_simulator_msgs::msg::BoundingBox & from_bounding_box, const CanonicalizedLaneletPose & to, const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, bool include_adjacent_lanelet, - bool include_opposite_direction, bool allow_lane_change, + bool include_opposite_direction, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> std::optional { if (const auto longitudinal_distance = longitudinalDistance( - from, to, include_adjacent_lanelet, include_opposite_direction, allow_lane_change, + from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr); longitudinal_distance) { const auto from_bounding_box_distances = diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index 36cf69ca25a..b354ea0ea0f 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -177,8 +177,8 @@ auto boundingBoxRelativePose( auto relativeLaneletPose( const CanonicalizedLaneletPose & from, const CanonicalizedLaneletPose & to, - const bool allow_lane_change, const std::shared_ptr & hdmap_utils_ptr) - -> LaneletPose + const traffic_simulator::RoutingConfiguration & routing_configuration, + const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{true}; @@ -188,11 +188,13 @@ auto relativeLaneletPose( // it is not possible to calculate one of them - it happens that one is sufficient if ( const auto longitudinal_distance = longitudinalDistance( - from, to, include_adjacent_lanelet, include_opposite_direction, allow_lane_change, + from, to, include_adjacent_lanelet, include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { position.s = longitudinal_distance.value(); } - if (const auto lateral_distance = lateralDistance(from, to, allow_lane_change, hdmap_utils_ptr)) { + if ( + const auto lateral_distance = + lateralDistance(from, to, routing_configuration, hdmap_utils_ptr)) { position.offset = lateral_distance.value(); } return position; @@ -202,7 +204,8 @@ 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 bool allow_lane_change, + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, + const traffic_simulator::RoutingConfiguration & routing_configuration, const std::shared_ptr & hdmap_utils_ptr) -> LaneletPose { constexpr bool include_adjacent_lanelet{false}; @@ -214,12 +217,12 @@ auto boundingBoxRelativeLaneletPose( if ( const auto longitudinal_bounding_box_distance = boundingBoxLaneLongitudinalDistance( from, from_bounding_box, to, to_bounding_box, include_adjacent_lanelet, - include_opposite_direction, allow_lane_change, hdmap_utils_ptr)) { + include_opposite_direction, routing_configuration, hdmap_utils_ptr)) { position.s = longitudinal_bounding_box_distance.value(); } if ( const auto lateral_bounding_box_distance = boundingBoxLaneLateralDistance( - from, from_bounding_box, to, to_bounding_box, allow_lane_change, hdmap_utils_ptr)) { + from, from_bounding_box, to, to_bounding_box, routing_configuration, hdmap_utils_ptr)) { position.offset = lateral_bounding_box_distance.value(); } return position; @@ -231,7 +234,7 @@ auto isInLanelet( { constexpr bool include_adjacent_lanelet{false}; constexpr bool include_opposite_direction{false}; - constexpr bool allow_lane_change{false}; + constexpr RoutingConfiguration routing_configuration; if (isSameLaneletId(canonicalized_lanelet_pose, lanelet_id)) { return true; @@ -240,7 +243,7 @@ auto isInLanelet( helper::constructCanonicalizedLaneletPose(lanelet_id, 0.0, 0.0, hdmap_utils_ptr); if (const auto distance_to_start_lanelet_pose = longitudinalDistance( start_lanelet_pose, canonicalized_lanelet_pose, include_adjacent_lanelet, - include_opposite_direction, allow_lane_change, hdmap_utils_ptr); + include_opposite_direction, routing_configuration, hdmap_utils_ptr); distance_to_start_lanelet_pose and std::abs(distance_to_start_lanelet_pose.value()) <= tolerance) { return true; @@ -250,7 +253,7 @@ auto isInLanelet( lanelet_id, hdmap_utils_ptr->getLaneletLength(lanelet_id), 0.0, hdmap_utils_ptr); if (const auto distance_to_end_lanelet_pose = longitudinalDistance( canonicalized_lanelet_pose, end_lanelet_pose, include_adjacent_lanelet, - include_opposite_direction, allow_lane_change, hdmap_utils_ptr); + include_opposite_direction, routing_configuration, hdmap_utils_ptr); distance_to_end_lanelet_pose and std::abs(distance_to_end_lanelet_pose.value()) <= tolerance) { return true; 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 904eaf66b0f..f161f368129 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 @@ -487,37 +487,47 @@ TEST_F(HdMapUtilsTest_StandardMap, CanonicalizeAll) TEST_F(HdMapUtilsTest_FourTrackHighwayMap, CountLaneChangesAlongRoute) { using traffic_simulator::helper::constructLaneletPose; + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(3002175, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(3002175, 0), + lane_changeable_routing_configuration), std::make_pair(1, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(3002182, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(3002182, 0), + lane_changeable_routing_configuration), std::make_pair(1, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(199, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(199, 0), + lane_changeable_routing_configuration), std::make_pair(1, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(3002176, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(3002176, 0), + lane_changeable_routing_configuration), std::make_pair(0, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(200, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(200, 0), + lane_changeable_routing_configuration), std::make_pair(0, 0)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(201, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(201, 0), + lane_changeable_routing_configuration), std::make_pair(0, 1)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(202, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(202, 0), + lane_changeable_routing_configuration), std::make_pair(0, 2)); EXPECT_EQ( hdmap_utils.countLaneChanges( - constructLaneletPose(3002176, 0), constructLaneletPose(206, 0), true), + constructLaneletPose(3002176, 0), constructLaneletPose(206, 0), + lane_changeable_routing_configuration), std::make_pair(0, 2)); } @@ -1724,7 +1734,7 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanNo EXPECT_FALSE(hdmap_utils .getLateralDistance( traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), - traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2), false) + traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2)) .has_value()); } @@ -1738,7 +1748,10 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanCh const auto from = traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5); const auto to = traffic_simulator::helper::constructLaneletPose(3002184, 10.0, 0.2); - const auto result = hdmap_utils.getLateralDistance(from, to, true); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = + hdmap_utils.getLateralDistance(from, to, lane_changeable_routing_configuration); EXPECT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 2.80373 / 2.0 + 3.03463 / 2.0 + to.offset - from.offset, 1e-3); @@ -1753,10 +1766,13 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_parallelLanesCanCh */ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_notConnected) { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_FALSE(hdmap_utils .getLateralDistance( traffic_simulator::helper::constructLaneletPose(3002185, 0.0, 0.5), - traffic_simulator::helper::constructLaneletPose(3002166, 10.0, 0.2), true) + traffic_simulator::helper::constructLaneletPose(3002166, 10.0, 0.2), + lane_changeable_routing_configuration) .has_value()); } @@ -1766,8 +1782,10 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLateralDistance_notConnected) */ TEST_F(HdMapUtilsTest_StandardMap, getRoute_correct) { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_EQ( - hdmap_utils.getRoute(34579, 34630, true), + hdmap_utils.getRoute(34579, 34630, lane_changeable_routing_configuration), (lanelet::Ids{34579, 34774, 120659, 120660, 34468, 34438, 34408, 34624, 34630})); } @@ -1780,11 +1798,12 @@ TEST_F(HdMapUtilsTest_StandardMap, getRoute_correctCache) { const lanelet::Id from_id = 34579; const lanelet::Id to_id = 34630; - const bool allow_lane_change = true; + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_EQ( - hdmap_utils.getRoute(from_id, to_id, allow_lane_change), - hdmap_utils.getRoute(from_id, to_id, allow_lane_change)); + hdmap_utils.getRoute(from_id, to_id, lane_changeable_routing_configuration), + hdmap_utils.getRoute(from_id, to_id, lane_changeable_routing_configuration)); } /** @@ -1794,7 +1813,11 @@ TEST_F(HdMapUtilsTest_StandardMap, getRoute_correctCache) */ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getRoute_impossibleRouting) { - EXPECT_EQ(hdmap_utils.getRoute(199, 196, true).size(), static_cast(0)); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + EXPECT_EQ( + hdmap_utils.getRoute(199, 196, lane_changeable_routing_configuration).size(), + static_cast(0)); } /** @@ -1807,7 +1830,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getRoute_circular) const lanelet::Id from_and_to_id = 120659; EXPECT_EQ( - hdmap_utils.getRoute(from_and_to_id, from_and_to_id, false), lanelet::Ids{from_and_to_id}); + hdmap_utils.getRoute(from_and_to_id, from_and_to_id, traffic_simulator::RoutingConfiguration()), + lanelet::Ids{from_and_to_id}); } /** @@ -2019,8 +2043,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLanelet) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto result_distance = - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false); + const auto result_distance = hdmap_utils.getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result_distance.has_value()); EXPECT_NEAR(result_distance.value(), 27.0, 1.0); @@ -2040,8 +2064,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_sameLaneletBehind) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto longitudinal_distance = - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false); + const auto longitudinal_distance = hdmap_utils.getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); EXPECT_FALSE(longitudinal_distance.has_value()); } @@ -2059,8 +2083,8 @@ TEST_F(HdMapUtilsTest_StandardMap, getLongitudinalDistance_differentLanelet) ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - const auto result_distance = - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false); + const auto result_distance = hdmap_utils.getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()); ASSERT_TRUE(result_distance.has_value()); EXPECT_NEAR(result_distance.value(), 86.0, 1.0); @@ -2080,8 +2104,10 @@ TEST_F(HdMapUtilsTest_FourTrackHighwayMap, getLongitudinalDistance_differentLane ASSERT_TRUE(pose_from.has_value()); ASSERT_TRUE(pose_to.has_value()); - EXPECT_FALSE( - hdmap_utils.getLongitudinalDistance(pose_from.value(), pose_to.value(), false).has_value()); + EXPECT_FALSE(hdmap_utils + .getLongitudinalDistance( + pose_from.value(), pose_to.value(), traffic_simulator::RoutingConfiguration()) + .has_value()); } /** @@ -2092,8 +2118,11 @@ TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348) auto pose_from = traffic_simulator::helper::constructLaneletPose(34468, 10.0); auto pose_to = traffic_simulator::helper::constructLaneletPose(34795, 5.0); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; EXPECT_NO_THROW(EXPECT_DOUBLE_EQ( - hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true).value(), + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, lane_changeable_routing_configuration) + .value(), 54.18867466433655977198213804513216018676757812500000)); } @@ -2105,14 +2134,19 @@ TEST_F(HdMapUtilsTest_KashiwanohaMap, getLongitudinalDistance_PullRequest1348) */ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + traffic_simulator::RoutingConfiguration default_routing_configuration; { const auto pose_from = traffic_simulator::helper::constructLaneletPose(563L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(659L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 157.0, 1.0); } @@ -2120,10 +2154,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(563L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(658L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 161.0, 1.0); } @@ -2131,10 +2167,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(563L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(657L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 161.0, 1.0); } @@ -2142,10 +2180,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(643L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(666L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 250.0, 1.0); } @@ -2153,10 +2193,12 @@ TEST_F(HdMapUtilsTest_IntersectionMap, getLongitudinalDistance_laneChange) const auto pose_from = traffic_simulator::helper::constructLaneletPose(643L, 5.0); const auto pose_to = traffic_simulator::helper::constructLaneletPose(665L, 5.0); - const auto without_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, false); + const auto without_lane_change = + hdmap_utils.getLongitudinalDistance(pose_from, pose_to, default_routing_configuration); EXPECT_FALSE(without_lane_change.has_value()); - const auto with_lane_change = hdmap_utils.getLongitudinalDistance(pose_from, pose_to, true); + const auto with_lane_change = hdmap_utils.getLongitudinalDistance( + pose_from, pose_to, lane_changeable_routing_configuration); ASSERT_TRUE(with_lane_change.has_value()); EXPECT_NEAR(with_lane_change.value(), 253.0, 1.0); } @@ -2739,11 +2781,16 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLanelets) TEST_F(HdMapUtilsTest_WithRoadShoulderMap, routingWithRoadShoulder) { // default: traffic_simulator::RoutingGraphType::VEHICLE - const auto default_route = hdmap_utils.getRoute(34693, 34615, false); + const auto default_route = + hdmap_utils.getRoute(34693, 34615, traffic_simulator::RoutingConfiguration()); EXPECT_EQ(default_route.size(), 0); - const auto route_with_road_shoulder = hdmap_utils.getRoute( - 34693, 34615, false, traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER); + traffic_simulator::RoutingConfiguration routing_configuration_with_road_shoulder; + routing_configuration_with_road_shoulder.routing_graph_type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER; + traffic_simulator::RoutingConfiguration default_routing_configuration; + const auto route_with_road_shoulder = + hdmap_utils.getRoute(34693, 34615, routing_configuration_with_road_shoulder); EXPECT_EQ(route_with_road_shoulder.size(), 4); EXPECT_EQ(route_with_road_shoulder[0], 34693); EXPECT_EQ(route_with_road_shoulder[1], 34696); // road shoulder diff --git a/simulation/traffic_simulator/test/src/utils/test_distance.cpp b/simulation/traffic_simulator/test/src/utils/test_distance.cpp index 8a2e20fdd12..ebe1558fb98 100644 --- a/simulation/traffic_simulator/test/src/utils/test_distance.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_distance.cpp @@ -92,12 +92,13 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_noChange) traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); { const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, std::numeric_limits::infinity(), false, hdmap_utils_ptr); + pose_from, pose_to, std::numeric_limits::infinity(), + traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, false, hdmap_utils_ptr); + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -115,13 +116,14 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_noChange) traffic_simulator::helper::constructCanonicalizedLaneletPose(201L, 0.0, 0.0, hdmap_utils_ptr); { const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, std::numeric_limits::infinity(), false, hdmap_utils_ptr); + pose_from, pose_to, std::numeric_limits::infinity(), + traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); } { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, false, hdmap_utils_ptr); + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, traffic_simulator::RoutingConfiguration(), hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 0.0, std::numeric_limits::epsilon()); } @@ -139,13 +141,18 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_change) const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + pose_from, pose_to, std::numeric_limits::infinity(), + lane_changeable_routing_configuration, hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, true, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, lane_changeable_routing_configuration, hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -164,14 +171,19 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_change) constexpr double approx_distance = -3.0; constexpr double tolerance = 0.5; { + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::lateralDistance( - pose_from, pose_to, std::numeric_limits::infinity(), true, hdmap_utils_ptr); + pose_from, pose_to, std::numeric_limits::infinity(), + lane_changeable_routing_configuration, hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), approx_distance, tolerance); } { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, true, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, lane_changeable_routing_configuration, hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), approx_distance, tolerance); } @@ -188,8 +200,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_impossible_matching) const auto pose_to = traffic_simulator::helper::constructCanonicalizedLaneletPose( 3002184L, 0.0, 0.0, hdmap_utils_ptr); { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, 2.0, true, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, 2.0, lane_changeable_routing_configuration, hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -206,8 +220,10 @@ TEST_F(distanceTest_FourTrackHighwayMap, lateralDistance_possible_matching) traffic_simulator::helper::constructCanonicalizedLaneletPose(202L, 0.0, 0.0, hdmap_utils_ptr); { - const auto result = - traffic_simulator::distance::lateralDistance(pose_from, pose_to, 3.0, true, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto result = traffic_simulator::distance::lateralDistance( + pose_from, pose_to, 3.0, lane_changeable_routing_configuration, hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), -3.0, 0.5); } @@ -229,7 +245,8 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, false, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -250,7 +267,8 @@ TEST_F(distanceTest_StandardMap, longitudinalDistance_noAdjacent_noOpposite_noCh ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, false, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 60.0, 1.0); } @@ -272,7 +290,8 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, false, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -293,7 +312,8 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_adjacent_noOpposit ASSERT_TRUE(pose_from.has_value()); const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, false, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, traffic_simulator::RoutingConfiguration(), + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 20.0, 1.0); } @@ -314,8 +334,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81599.34, 50022.34, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { @@ -326,8 +349,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81612.95, 49991.30, 280.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -347,8 +373,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81570.56, 50141.75, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 145.0, 1.0); } @@ -360,8 +389,11 @@ TEST_F(distanceTest_FourTrackHighwayMap, longitudinalDistance_noAdjacent_noOppos makePose(81610.25, 49988.59, 100.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 178.0, 1.0); } @@ -380,8 +412,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 118.0, 1.0)); } { @@ -392,8 +427,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86647.23, 44882.51, 240.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 195.0, 1.0)); } { @@ -404,8 +442,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86553.48, 44990.56, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 257.0, 1.0)); } { @@ -416,8 +457,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_noAdjacent_noOpposite_ makePose(86579.91, 44979.00, 150.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), false, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), false, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_NO_THROW(EXPECT_NEAR(result.value(), 228.0, 1.0)); } } @@ -437,8 +481,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch makePose(86642.95, 44958.78, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } { @@ -449,8 +496,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch makePose(86704.59, 44927.32, 340.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); EXPECT_FALSE(result.has_value()); } } @@ -468,8 +518,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch const auto pose_to = traffic_simulator::toCanonicalizedLaneletPose( makePose(86648.82, 44886.19, 240.0), false, hdmap_utils_ptr); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 103.0, 1.0); } @@ -481,8 +534,11 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch makePose(86599.32, 44975.01, 180.0), false, hdmap_utils_ptr); ASSERT_TRUE(pose_from.has_value()); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; const auto result = traffic_simulator::distance::longitudinalDistance( - pose_from.value(), pose_to.value(), true, false, true, hdmap_utils_ptr); + pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration, + hdmap_utils_ptr); ASSERT_TRUE(result.has_value()); EXPECT_NEAR(result.value(), 131.0, 1.0); } diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index a7132a31c62..78c4c107294 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -423,7 +423,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_invalid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.s)); } @@ -438,7 +439,8 @@ TEST_F(PoseTest, relativeLaneletPose_s_valid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_NEAR(relative.s, 107.74, 0.001); } @@ -453,7 +455,8 @@ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.offset)); } @@ -468,7 +471,10 @@ TEST_F(PoseTest, relativeLaneletPose_offset_valid) const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); - const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, true, hdmap_utils); + traffic_simulator::RoutingConfiguration lane_changeable_routing_configuration; + lane_changeable_routing_configuration.allow_lane_change = true; + const auto relative = traffic_simulator::pose::relativeLaneletPose( + from, to, lane_changeable_routing_configuration, hdmap_utils); EXPECT_EQ(relative.offset, 1.0); } @@ -485,7 +491,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.s)); } @@ -502,7 +508,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_NEAR(relative.s, 103.74, 0.01); } @@ -519,7 +525,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_TRUE(std::isnan(relative.s)); } @@ -536,7 +542,7 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, bounding_box, to, bounding_box, false, hdmap_utils); + from, bounding_box, to, bounding_box, traffic_simulator::RoutingConfiguration(), hdmap_utils); EXPECT_EQ(relative.offset, 0.0); } diff --git a/simulation/traffic_simulator_msgs/CHANGELOG.rst b/simulation/traffic_simulator_msgs/CHANGELOG.rst index 57fada0d6dc..4804733b6bc 100644 --- a/simulation/traffic_simulator_msgs/CHANGELOG.rst +++ b/simulation/traffic_simulator_msgs/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package openscenario_msgs * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/simulation/traffic_simulator_msgs/package.xml b/simulation/traffic_simulator_msgs/package.xml index 00b6345f0be..261f49d7b6a 100644 --- a/simulation/traffic_simulator_msgs/package.xml +++ b/simulation/traffic_simulator_msgs/package.xml @@ -2,7 +2,7 @@ traffic_simulator_msgs - 5.4.0 + 6.0.1 ROS messages for openscenario Masaya Kataoka Apache License 2.0 diff --git a/test_runner/random_test_runner/CHANGELOG.rst b/test_runner/random_test_runner/CHANGELOG.rst index 89c781931e2..8d799cc9f3e 100644 --- a/test_runner/random_test_runner/CHANGELOG.rst +++ b/test_runner/random_test_runner/CHANGELOG.rst @@ -21,6 +21,18 @@ Changelog for package random_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/test_runner/random_test_runner/package.xml b/test_runner/random_test_runner/package.xml index 0e154d24f87..6118e285567 100644 --- a/test_runner/random_test_runner/package.xml +++ b/test_runner/random_test_runner/package.xml @@ -2,7 +2,7 @@ random_test_runner - 5.4.0 + 6.0.1 Random behavior test runner piotr-zyskowski-rai Apache License 2.0 diff --git a/test_runner/scenario_test_runner/CHANGELOG.rst b/test_runner/scenario_test_runner/CHANGELOG.rst index f3b808a16ca..b8ea5020cdb 100644 --- a/test_runner/scenario_test_runner/CHANGELOG.rst +++ b/test_runner/scenario_test_runner/CHANGELOG.rst @@ -35,6 +35,18 @@ Changelog for package scenario_test_runner * Merge remote-tracking branch 'origin/master' into feature/publish_empty_context * Contributors: Masaya Kataoka +6.0.1 (2024-11-27) +------------------ + +6.0.0 (2024-11-27) +------------------ +* Merge branch 'master' into refactor/add_routing_graph_argument +* Merge branch 'master' into refactor/add_routing_graph_argument +* Contributors: Kotaro Yoshimoto + +5.5.0 (2024-11-27) +------------------ + 5.4.0 (2024-11-26) ------------------ * Merge branch 'master' into feature/shoulder_routing_graph diff --git a/test_runner/scenario_test_runner/package.xml b/test_runner/scenario_test_runner/package.xml index 8d81f5acaa5..7cba08daa13 100644 --- a/test_runner/scenario_test_runner/package.xml +++ b/test_runner/scenario_test_runner/package.xml @@ -2,7 +2,7 @@ scenario_test_runner - 5.4.0 + 6.0.1 scenario test runner package Tatsuya Yamasaki Apache License 2.0