From f0a30db3e1248ca70b21e0263ec49f394f622ed9 Mon Sep 17 00:00:00 2001 From: Dawid Moszynski Date: Wed, 27 Nov 2024 17:52:55 +0100 Subject: [PATCH] fix(traffic_simulator): fix lanelet_wrapper to pass tests --- .../lanelet_wrapper/lanelet_wrapper.hpp | 10 ++--- .../src/lanelet_wrapper/lanelet_wrapper.cpp | 6 +-- .../src/lanelet_wrapper/pose.cpp | 2 - .../src/lanelet_wrapper/route.cpp | 43 +++++++++++++++++++ .../traffic_simulator/src/utils/distance.cpp | 1 - 5 files changed, 51 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp index 1f4fbdb78a0..801120f590f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp @@ -243,7 +243,7 @@ class LaneletLengthCache struct TrafficRulesWithRoutingGraph { lanelet::traffic_rules::TrafficRulesPtr rules; - lanelet::routing::RoutingGraphPtr graph; + lanelet::routing::RoutingGraphConstPtr graph; mutable RouteCache route_cache; }; @@ -252,18 +252,18 @@ class LaneletWrapper public: static auto activate(const std::string & lanelet_map_path) -> void; - [[nodiscard]] static auto map() -> const lanelet::LaneletMapPtr &; + [[nodiscard]] static auto map() -> const lanelet::LaneletMapPtr; [[nodiscard]] static auto routingGraph(const RoutingGraphType type) - -> const lanelet::routing::RoutingGraphConstPtr &; + -> const lanelet::routing::RoutingGraphConstPtr; [[nodiscard]] static auto trafficRules(const RoutingGraphType type) - -> const lanelet::traffic_rules::TrafficRulesPtr &; + -> const lanelet::traffic_rules::TrafficRulesPtr; [[nodiscard]] static auto routeCache(const RoutingGraphType type) -> RouteCache &; [[nodiscard]] static auto centerPointsCache() -> CenterPointsCache &; [[nodiscard]] static auto laneletLengthCache() -> LaneletLengthCache &; private: - LaneletWrapper(const std::filesystem::path & lanelet2_map_path); + LaneletWrapper(const std::filesystem::path & lanelet_map_path); static LaneletWrapper & getInstance(); auto calculateAccumulatedLengths(const lanelet::ConstLineString3d & line_string) diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp index 8a7144e1c3c..9356b902e50 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_wrapper.cpp @@ -46,13 +46,13 @@ LaneletWrapper & LaneletWrapper::getInstance() return *instance; } -auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr & +auto LaneletWrapper::map() -> const lanelet::LaneletMapPtr { return getInstance().lanelet_map_ptr_; } auto LaneletWrapper::routingGraph(const RoutingGraphType type) - -> const lanelet::routing::RoutingGraphConstPtr & + -> const lanelet::routing::RoutingGraphConstPtr { switch (type) { case RoutingGraphType::VEHICLE: @@ -69,7 +69,7 @@ auto LaneletWrapper::routingGraph(const RoutingGraphType type) } auto LaneletWrapper::trafficRules(const RoutingGraphType type) - -> const lanelet::traffic_rules::TrafficRulesPtr & + -> const lanelet::traffic_rules::TrafficRulesPtr { switch (type) { case RoutingGraphType::VEHICLE: diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp index 532d1bba21b..d3dbca38ffd 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp @@ -170,8 +170,6 @@ auto toLaneletPoses( auto insertIds = [&](const auto & new_ids) { lanelet_ids_set.insert(new_ids.begin(), new_ids.end()); }; - /// @todo here entity_type is hardcoded as VEHICLE - const auto entity_type = traffic_simulator_msgs::build().type(EntityType::VEHICLE); insertIds(leftLaneletIds(lanelet_id, type, include_opposite_direction)); insertIds(rightLaneletIds(lanelet_id, type, include_opposite_direction)); insertIds(lanelet_map::previousLaneletIds({lanelet_id}, type)); diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp index 12ebe8467e6..49e6cc4e4c7 100644 --- a/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp +++ b/simulation/traffic_simulator/src/lanelet_wrapper/route.cpp @@ -64,6 +64,49 @@ auto route( LaneletWrapper::routingGraph(routing_configuration.routing_graph_type)); } +// auto route( +// const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, +// const RoutingConfiguration & routing_configuration) -> lanelet::Ids +// { +// auto & cache = LaneletWrapper::routeCache(routing_configuration.routing_graph_type); +// std::cout << " ########## A ##########" << std::endl; +// 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); +// } +// std::cout << " ########## B ##########" << std::endl; + +// lanelet::Ids ids; +// const auto lanelet = LaneletWrapper::map()->laneletLayer.get(from_lanelet_id); +// const auto to_lanelet = LaneletWrapper::map()->laneletLayer.get(to_lanelet_id); +// std::cout << " ########## B + ##########" << std::endl; +// const auto graph = LaneletWrapper::routingGraph(routing_configuration.routing_graph_type); +// std::cout << " ########## B ++ ##########" << std::endl; +// lanelet::Optional route = +// graph->getRoute(lanelet, to_lanelet, 0, routing_configuration.allow_lane_change); +// std::cout << " ########## C ##########" << std::endl; + +// if (!route) { +// cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); +// return ids; +// } +// lanelet::routing::LaneletPath shortest_path = route->shortestPath(); +// std::cout << " ########## D ##########" << std::endl; + +// if (shortest_path.empty()) { +// cache.appendData(from_lanelet_id, to_lanelet_id, routing_configuration.allow_lane_change, ids); +// return ids; +// } +// std::cout << " ########## E ##########" << std::endl; + +// 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, routing_configuration.allow_lane_change, ids); +// std::cout << " ########## G ##########" << std::endl; + +// return ids; +// } + auto followingLanelets( const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon, const bool include_current_lanelet_id, const RoutingGraphType type) -> lanelet::Ids diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp index ad9215fe0cf..3dfba19eef0 100644 --- a/simulation/traffic_simulator/src/utils/distance.cpp +++ b/simulation/traffic_simulator/src/utils/distance.cpp @@ -275,7 +275,6 @@ auto distanceToYieldStop( const CanonicalizedLaneletPose & reference_pose, const lanelet::Ids & following_lanelets, const std::vector & other_poses) -> std::optional { - constexpr bool allow_lane_change{false}; auto getPosesOnLanelet = [&other_poses](const auto & lanelet_id) { std::vector ret; for (const auto & pose : other_poses) {