From 47614906b557c22a00631efcb898c8f50dc2f51a Mon Sep 17 00:00:00 2001 From: abco20 Date: Mon, 17 Feb 2025 15:25:41 +0900 Subject: [PATCH 1/5] Implement static route function in lanelet_wrapper --- simulation/traffic_simulator/CMakeLists.txt | 1 + .../lanelet_wrapper/route.hpp | 34 ++++++++++++++ .../src/lanelet_wrapper/route.cpp | 46 +++++++++++++++++++ 3 files changed, 81 insertions(+) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp create mode 100644 simulation/traffic_simulator/src/lanelet_wrapper/route.cpp diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index f55539afe1f..ffdf947bbad 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -52,6 +52,7 @@ ament_auto_add_library(traffic_simulator SHARED src/lanelet_wrapper/lanelet_map.cpp src/lanelet_wrapper/lanelet_wrapper.cpp src/lanelet_wrapper/pose.cpp + src/lanelet_wrapper/route.cpp src/lanelet_wrapper/traffic_rules.cpp src/simulation_clock/simulation_clock.cpp src/traffic/traffic_controller.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp new file mode 100644 index 00000000000..ead1d5ccc8a --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp @@ -0,0 +1,34 @@ +// Copyright 2015 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ +#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ + +#include + +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace route +{ +auto route( + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const RoutingConfiguration & routing_configuration = RoutingConfiguration()) -> lanelet::Ids; +} // namespace route +} // namespace lanelet_wrapper +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ \ No newline at end of file diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp new file mode 100644 index 00000000000..26930ba5ee0 --- /dev/null +++ b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp @@ -0,0 +1,46 @@ + + +// Copyright 2015 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +namespace lanelet_wrapper +{ +namespace route +{ +auto route( + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, + const RoutingConfiguration & routing_configuration) -> lanelet::Ids +{ + /// @todo improve architecture in terms of access to cache of the graph + return LaneletWrapper::routeCache(routing_configuration.routing_graph_type) + .getRoute( + from_lanelet_id, to_lanelet_id, LaneletWrapper::map(), routing_configuration, + LaneletWrapper::routingGraph(routing_configuration.routing_graph_type)); +} +} // namespace route +} // namespace lanelet_wrapper +} // namespace traffic_simulator \ No newline at end of file From 6a919dd1753a9c42845a21ed7ad6c4d5a558d3af Mon Sep 17 00:00:00 2001 From: abco20 Date: Mon, 17 Feb 2025 15:42:34 +0900 Subject: [PATCH 2/5] add route::route utils --- .../include/traffic_simulator/utils/route.hpp | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp new file mode 100644 index 00000000000..7af9283a435 --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp @@ -0,0 +1,36 @@ +// Copyright 2015 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ +#define TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace traffic_simulator +{ +inline namespace route +{ +template +auto route(Ts &&... xs) +{ + return lanelet_wrapper::route::route(std::forward(xs)...); +} +} // namespace route +} // namespace traffic_simulator +#endif // TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ \ No newline at end of file From 7086f4e2391ba41600819f2ca2215090c491f7e9 Mon Sep 17 00:00:00 2001 From: abco20 Date: Mon, 17 Feb 2025 15:56:04 +0900 Subject: [PATCH 3/5] use non member route function `findRoutableAlternativeLaneletPoseFrom` --- .../behavior_tree_plugin/src/action_node.cpp | 2 +- .../include/traffic_simulator/utils/pose.hpp | 3 +-- simulation/traffic_simulator/src/utils/pose.cpp | 17 ++++++++--------- 3 files changed, 10 insertions(+), 12 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp index 5f26d0eca7b..80a685d79da 100644 --- a/simulation/behavior_tree_plugin/src/action_node.cpp +++ b/simulation/behavior_tree_plugin/src/action_node.cpp @@ -334,7 +334,7 @@ auto ActionNode::getDistanceToTargetEntity( if (const auto & target_lanelet_pose = traffic_simulator::pose::findRoutableAlternativeLaneletPoseFrom( canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(), - target_bounding_box, hdmap_utils); + target_bounding_box); target_lanelet_pose) { const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose(); const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox(); diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp index dca28748e19..96712c7487a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp @@ -113,8 +113,7 @@ auto isAtEndOfLanelets( auto findRoutableAlternativeLaneletPoseFrom( const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & canonicalized_lanelet_pose, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, - const std::shared_ptr & hdmap_utils_ptr) + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box) -> std::optional; namespace pedestrian diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp index a2467ca1b0a..65ad164003b 100644 --- a/simulation/traffic_simulator/src/utils/pose.cpp +++ b/simulation/traffic_simulator/src/utils/pose.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace traffic_simulator @@ -386,8 +387,7 @@ auto isAtEndOfLanelets( auto findRoutableAlternativeLaneletPoseFrom( const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & to_canonicalized_lanelet_pose, - const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box, - const std::shared_ptr & hdmap_utils_ptr) + const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box) -> std::optional { /// @note search_distance should be minimal, just to check nearest neighbour lanelets @@ -395,17 +395,17 @@ auto findRoutableAlternativeLaneletPoseFrom( /// @note default_match_to_lane_reduction_ratio is constant described in hdmap_utils.hpp constexpr auto default_match_to_lane_reduction_ratio{0.8}; constexpr auto include_crosswalk{false}; - /** - * @note hdmap_utils_ptr->getRoute requires routing_configuration, - * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets - */ + /** + * @note route::route requires routing_configuration, + * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets + */ RoutingConfiguration routing_configuration; routing_configuration.allow_lane_change = true; /// @note if there is already a route from_lanelet_id->to_lanelet_id, return it /// if not, transform the 'to_lanelet_id' position into the nearby lanelets and search for a route in relation to them if (const auto to_lanelet_id = to_canonicalized_lanelet_pose.getLaneletPose().lanelet_id; - !hdmap_utils_ptr->getRoute(from_lanelet_id, to_lanelet_id, routing_configuration).empty()) { + !route::route(from_lanelet_id, to_lanelet_id, routing_configuration).empty()) { return to_canonicalized_lanelet_pose; } else if (const auto nearby_lanelet_ids = lanelet_wrapper::pose::findMatchingLanes( static_cast(to_canonicalized_lanelet_pose), @@ -414,8 +414,7 @@ auto findRoutableAlternativeLaneletPoseFrom( nearby_lanelet_ids.has_value()) { std::vector> routes; for (const auto & [distance, lanelet_id] : nearby_lanelet_ids.value()) { - if (auto route = - hdmap_utils_ptr->getRoute(from_lanelet_id, lanelet_id, routing_configuration); + if (auto route = route::route(from_lanelet_id, lanelet_id, routing_configuration); lanelet_id == to_lanelet_id || route.empty()) { continue; } else if (const auto lanelet_pose = lanelet_wrapper::pose::toLaneletPose( From 703427bbb3a9152329c582ce92c76e0c841788dd Mon Sep 17 00:00:00 2001 From: abco20 Date: Mon, 17 Feb 2025 15:56:13 +0900 Subject: [PATCH 4/5] use non member route function `getAlternativeLaneletPoseBaseOnShortestRouteFrom` --- .../traffic_simulator/data_type/lanelet_pose.hpp | 2 +- .../traffic_simulator/src/data_type/lanelet_pose.cpp | 12 ++++++------ simulation/traffic_simulator/src/utils/distance.cpp | 2 +- .../test/src/data_type/test_lanelet_pose.cpp | 12 ++++++------ 4 files changed, 14 insertions(+), 14 deletions(-) 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 90e2fb11a2e..25ae7b4d7e7 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 @@ -49,7 +49,7 @@ class CanonicalizedLaneletPose auto alignOrientationToLanelet() -> void; auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; } auto getAlternativeLaneletPoseBaseOnShortestRouteFrom( - LaneletPose from, const std::shared_ptr & hdmap_utils, + LaneletPose from, const RoutingConfiguration & routing_configuration = RoutingConfiguration()) const -> std::optional; diff --git a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp index 121bd44af5c..351372c7e68 100644 --- a/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp +++ b/simulation/traffic_simulator/src/data_type/lanelet_pose.cpp @@ -20,6 +20,7 @@ #include #include #include +#include namespace traffic_simulator { @@ -67,18 +68,17 @@ auto CanonicalizedLaneletPose::operator=(const CanonicalizedLaneletPose & other) } auto CanonicalizedLaneletPose::getAlternativeLaneletPoseBaseOnShortestRouteFrom( - LaneletPose from, const std::shared_ptr & hdmap_utils, - const RoutingConfiguration & routing_configuration) const -> std::optional + LaneletPose from, const 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, routing_configuration); + auto shortest_route = + route::route(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, routing_configuration); + const auto route = route::route(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/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index 880a9cbfa5e..8e5f7e336fb 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -68,7 +68,7 @@ auto longitudinalDistance( if (to.hasAlternativeLaneletPose()) { if ( const auto to_canonicalized_opt = to.getAlternativeLaneletPoseBaseOnShortestRouteFrom( - static_cast(from), hdmap_utils_ptr, routing_configuration)) { + static_cast(from), routing_configuration)) { to_canonicalized = to_canonicalized_opt.value(); } } diff --git a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp index eb03e9970d9..7f3fc4fb15c 100644 --- a/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp +++ b/simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp @@ -191,8 +191,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); - const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); - const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2); ASSERT_TRUE(result1.has_value()); ASSERT_TRUE(result2.has_value()); @@ -210,8 +210,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); - const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); - const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2); ASSERT_TRUE(result1.has_value()); ASSERT_TRUE(result2.has_value()); @@ -230,8 +230,8 @@ TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRout const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0); const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0); - const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils); - const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils); + const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1); + const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2); ASSERT_TRUE(result1.has_value()); ASSERT_TRUE(result2.has_value()); From e1685ea908227ebfe0bc653026102d809fbed678 Mon Sep 17 00:00:00 2001 From: abco20 Date: Mon, 17 Feb 2025 16:05:41 +0900 Subject: [PATCH 5/5] fix format --- .../include/traffic_simulator/lanelet_wrapper/route.hpp | 2 +- .../include/traffic_simulator/utils/route.hpp | 2 +- simulation/traffic_simulator/src/lanelet_wrapper/route.cpp | 3 +-- 3 files changed, 3 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp index ead1d5ccc8a..9d1d8728c7d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/route.hpp @@ -31,4 +31,4 @@ auto route( } // namespace route } // namespace lanelet_wrapper } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ \ No newline at end of file +#endif // TRAFFIC_SIMULATOR__LANELET_WRAPPER_ROUTE_HPP_ diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp index 7af9283a435..db6570a3c97 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/utils/route.hpp @@ -33,4 +33,4 @@ auto route(Ts &&... xs) } } // namespace route } // namespace traffic_simulator -#endif // TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ \ No newline at end of file +#endif // TRAFFIC_SIMULATOR__UTILS__ROUTE_HPP_ diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp index 26930ba5ee0..60b931bfb47 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp @@ -1,5 +1,4 @@ - // Copyright 2015 Tier IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -43,4 +42,4 @@ auto route( } } // namespace route } // namespace lanelet_wrapper -} // namespace traffic_simulator \ No newline at end of file +} // namespace traffic_simulator