From 7b718a88c28bf5177494300b0bf17b6a3540bb97 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 20 Nov 2024 16:14:08 +0900 Subject: [PATCH 01/12] feat: add new traffic rule "germanRoadShoulderPassableVehicleRules" --- simulation/traffic_simulator/CMakeLists.txt | 1 + .../hdmap_utils/traffic_rules.hpp | 71 +++++++++++++++++++ .../src/hdmap_utils/traffic_rules.cpp | 21 ++++++ 3 files changed, 93 insertions(+) create mode 100644 simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp create mode 100644 simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp diff --git a/simulation/traffic_simulator/CMakeLists.txt b/simulation/traffic_simulator/CMakeLists.txt index a143180fe36..9394ec74af8 100644 --- a/simulation/traffic_simulator/CMakeLists.txt +++ b/simulation/traffic_simulator/CMakeLists.txt @@ -45,6 +45,7 @@ ament_auto_add_library(traffic_simulator SHARED src/entity/pedestrian_entity.cpp src/entity/vehicle_entity.cpp src/hdmap_utils/hdmap_utils.cpp + src/hdmap_utils/traffic_rules.cpp src/helper/helper.cpp src/job/job_list.cpp src/job/job.cpp diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp new file mode 100644 index 00000000000..a655fcc1d7d --- /dev/null +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp @@ -0,0 +1,71 @@ +// 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__HDMAP_UTILS__TRAFFIC_RULES_HPP_ +#define TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_ + +#include + +namespace hdmap_utils +{ +struct Locations +{ + /// @note DIRTY HACK!! Originally, a location code should be an ISO country code. + /// @sa https://github.com/fzi-forschungszentrum-informatik/Lanelet2/blob/507033b82d9915f086f2539d56c3b62e71802438/lanelet2_traffic_rules/include/lanelet2_traffic_rules/TrafficRules.h#L97 + static constexpr char RoadShoulderPassableGermany[] = "de_road_shoulder_passable"; +}; + +class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanVehicle +{ +public: + using lanelet::traffic_rules::GermanVehicle::GermanVehicle; + +protected: + lanelet::Optional canPass( + const std::string & type, const std::string & /*location*/) const override + { + using ParticantsMap = std::map>; + using Value = lanelet::AttributeValueString; + using Participants = lanelet::Participants; + const static ParticantsMap ParticipantMap{ + // clang-format off + {"", {Participants::Vehicle}}, + {Value::Road, {Participants::Vehicle, Participants::Bicycle}}, + {"road_shoulder", {Participants::Vehicle, Participants::Bicycle}}, // add road_shoulder + {Value::Highway, {Participants::Vehicle}}, + {Value::BicycleLane, {Participants::Bicycle}}, + {Value::PlayStreet, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, + {Value::EmergencyLane, {Participants::VehicleEmergency}}, + {Value::Exit, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, + {Value::Walkway, {Participants::Pedestrian}}, + {Value::Crosswalk, {Participants::Pedestrian}}, + {Value::Stairs, {Participants::Pedestrian}}, + {Value::SharedWalkway, {Participants::Pedestrian, Participants::Bicycle}} + // clang-format on + }; + auto participants = ParticipantMap.find(type); + if (participants == ParticipantMap.end()) { + return {}; + } + + auto startswith = [](const std::string & str, const std::string & substr) { + return str.compare(0, substr.size(), substr) == 0; + }; + return lanelet::utils::anyOf(participants->second, [this, startswith](auto & participant) { + return startswith(this->participant(), participant); + }); + } +}; +} // namespace hdmap_utils +#endif // TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_ diff --git a/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp b/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp new file mode 100644 index 00000000000..2140970be62 --- /dev/null +++ b/simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp @@ -0,0 +1,21 @@ +// 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 + +lanelet::traffic_rules::RegisterTrafficRules + germanRoadShoulderPassableVehicleRules( + hdmap_utils::Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); From 9535de62f0e07ec39c7b252e7919757562a1de76 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 21 Nov 2024 16:03:07 +0900 Subject: [PATCH 02/12] chore: fix typo --- .../include/traffic_simulator/hdmap_utils/traffic_rules.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp index a655fcc1d7d..3db4ce6237f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp @@ -35,10 +35,10 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV lanelet::Optional canPass( const std::string & type, const std::string & /*location*/) const override { - using ParticantsMap = std::map>; + using ParticipantsMap = std::map>; using Value = lanelet::AttributeValueString; using Participants = lanelet::Participants; - const static ParticantsMap ParticipantMap{ + const static ParticipantsMap ParticipantMap{ // clang-format off {"", {Participants::Vehicle}}, {Value::Road, {Participants::Vehicle, Participants::Bicycle}}, From 0122778c9041fa7e401c5f50c6aeeac8522fbbae Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 21 Nov 2024 16:03:15 +0900 Subject: [PATCH 03/12] chore: fix cspell error --- .../include/traffic_simulator/hdmap_utils/traffic_rules.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp index 3db4ce6237f..cdad52fe9ef 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp @@ -59,6 +59,7 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV return {}; } + // cspell: ignore startswith auto startswith = [](const std::string & str, const std::string & substr) { return str.compare(0, substr.size(), substr) == 0; }; From a31c5f0a9e26217a687dba7b0544e1db570b0d8a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 21 Nov 2024 16:19:54 +0900 Subject: [PATCH 04/12] feat: add RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER --- .../traffic_simulator/data_type/routing_graph_type.hpp | 2 +- .../traffic_simulator/hdmap_utils/hdmap_utils.hpp | 3 +++ .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 10 ++++++++++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_graph_type.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_graph_type.hpp index f67906f9bc3..3e34ae07a46 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_graph_type.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/routing_graph_type.hpp @@ -21,7 +21,7 @@ namespace traffic_simulator { inline namespace routing_graph_type { -enum class RoutingGraphType : std::uint8_t { VEHICLE, PEDESTRIAN }; +enum class RoutingGraphType : std::uint8_t { VEHICLE, PEDESTRIAN, VEHICLE_WITH_ROAD_SHOULDER }; char const * to_string(const RoutingGraphType &); } // namespace routing_graph_type 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 42c588c427b..2c8e903e59c 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 @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -367,6 +368,8 @@ class HdMapUtils RuleWithGraph vehicle; + RuleWithGraph vehicle_with_road_shoulder; + RuleWithGraph pedestrian; }; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 002b69dde3c..142e077528a 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -2199,6 +2199,10 @@ HdMapUtils::RoutingGraphs::RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_ vehicle.rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Vehicle); vehicle.graph = lanelet::routing::RoutingGraph::build(*lanelet_map, *vehicle.rules); + vehicle_with_road_shoulder.rules = lanelet::traffic_rules::TrafficRulesFactory::create( + Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle); + vehicle_with_road_shoulder.graph = + lanelet::routing::RoutingGraph::build(*lanelet_map, *vehicle_with_road_shoulder.rules); pedestrian.rules = lanelet::traffic_rules::TrafficRulesFactory::create( lanelet::Locations::Germany, lanelet::Participants::Pedestrian); pedestrian.graph = lanelet::routing::RoutingGraph::build(*lanelet_map, *pedestrian.rules); @@ -2210,6 +2214,8 @@ lanelet::routing::RoutingGraphPtr HdMapUtils::RoutingGraphs::routing_graph( switch (type) { case traffic_simulator::RoutingGraphType::VEHICLE: return vehicle.graph; + case traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return vehicle_with_road_shoulder.graph; case traffic_simulator::RoutingGraphType::PEDESTRIAN: return pedestrian.graph; default: @@ -2225,6 +2231,8 @@ lanelet::traffic_rules::TrafficRulesPtr HdMapUtils::RoutingGraphs::traffic_rule( switch (type) { case traffic_simulator::RoutingGraphType::VEHICLE: return vehicle.rules; + case traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return vehicle_with_road_shoulder.rules; case traffic_simulator::RoutingGraphType::PEDESTRIAN: return pedestrian.rules; default: @@ -2239,6 +2247,8 @@ RouteCache & HdMapUtils::RoutingGraphs::route_cache(const traffic_simulator::Rou switch (type) { case traffic_simulator::RoutingGraphType::VEHICLE: return vehicle.route_cache; + case traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER: + return vehicle_with_road_shoulder.route_cache; case traffic_simulator::RoutingGraphType::PEDESTRIAN: return pedestrian.route_cache; default: From 0c46fa40a4ed43a27d7198f23296330b5e7a92c2 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 14:32:27 +0900 Subject: [PATCH 05/12] refactor: add routing graph argument and stop giving special treatment to shoulder_lanelet in HdMapUtils::get{Next|Previous}LaneletIds --- .../hdmap_utils/hdmap_utils.hpp | 10 ++++++-- .../src/hdmap_utils/hdmap_utils.cpp | 24 +++++++------------ 2 files changed, 16 insertions(+), 18 deletions(-) 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 2c8e903e59c..7cc9bd71f46 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 @@ -208,7 +208,10 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; - auto getNextLaneletIds(const lanelet::Id) const -> lanelet::Ids; + auto getNextLaneletIds( + const lanelet::Id, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const + -> lanelet::Ids; auto getNextLaneletIds( const lanelet::Id, const std::string & turn_direction, @@ -222,7 +225,10 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; - auto getPreviousLaneletIds(const lanelet::Id) const -> lanelet::Ids; + auto getPreviousLaneletIds( + const lanelet::Id, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const + -> lanelet::Ids; auto getPreviousLaneletIds( const lanelet::Id, const std::string & turn_direction, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 142e077528a..253f8ae4961 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -993,19 +993,15 @@ auto HdMapUtils::getPreviousRoadShoulderLanelet(const lanelet::Id lanelet_id) co return ids; } -/// @todo add RoutingGraphType argument after supporting a routing graph with road shoulder lanelets -auto HdMapUtils::getPreviousLaneletIds(const lanelet::Id lanelet_id) const -> lanelet::Ids +auto HdMapUtils::getPreviousLaneletIds( + const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : - routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->previous(lanelet)) { + for (const auto & llt : routing_graphs_->routing_graph(type)->previous(lanelet)) { ids.push_back(llt.id()); } - for (const auto & id : getPreviousRoadShoulderLanelet(lanelet_id)) { - ids.push_back(id); - } return ids; } @@ -1056,19 +1052,15 @@ auto HdMapUtils::getNextRoadShoulderLanelet(const lanelet::Id lanelet_id) const return ids; } -/// @todo add RoutingGraphType argument after supporting a routing graph with road shoulder lanelets -auto HdMapUtils::getNextLaneletIds(const lanelet::Id lanelet_id) const -> lanelet::Ids +auto HdMapUtils::getNextLaneletIds( + const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { lanelet::Ids ids; const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & llt : - routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE) - ->following(lanelet)) { + for (const auto & llt : routing_graphs_->routing_graph(type)->following(lanelet)) { ids.push_back(llt.id()); } - for (const auto & id : getNextRoadShoulderLanelet(lanelet_id)) { - ids.push_back(id); - } return ids; } From 4362e83ca95bde7ebc69d8c8155e4d4ecffe1f50 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 15:01:51 +0900 Subject: [PATCH 06/12] refactor: delete HdMapUtils::shoulder_lanelet --- .../hdmap_utils/hdmap_utils.hpp | 12 +++------ .../src/hdmap_utils/hdmap_utils.cpp | 26 ------------------- 2 files changed, 4 insertions(+), 34 deletions(-) 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 7cc9bd71f46..90214542b6a 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 @@ -201,7 +201,8 @@ class HdMapUtils const geometry_msgs::msg::Point &, const double distance_threshold, const std::size_t search_count = 5) const -> lanelet::Ids; - auto getNextLaneletIds(const lanelet::Ids &) const -> lanelet::Ids; + auto getNextLaneletIds(const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const -> lanelet::Ids; auto getNextLaneletIds( const lanelet::Ids &, const std::string & turn_direction, @@ -218,7 +219,8 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; - auto getPreviousLaneletIds(const lanelet::Ids &) const -> lanelet::Ids; + auto getPreviousLaneletIds(const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const -> lanelet::Ids; auto getPreviousLaneletIds( const lanelet::Ids &, const std::string & turn_direction, @@ -344,8 +346,6 @@ class HdMapUtils lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::ConstLanelets shoulder_lanelets_; - class RoutingGraphs { public: @@ -421,10 +421,6 @@ class HdMapUtils const traffic_simulator::lane_change::TrajectoryShape, const double tangent_vector_size = 100) const -> math::geometry::HermiteCurve; - auto getNextRoadShoulderLanelet(const lanelet::Id) const -> lanelet::Ids; - - auto getPreviousRoadShoulderLanelet(const lanelet::Id) const -> lanelet::Ids; - auto getStopLines() const -> lanelet::ConstLineStrings3d; auto getStopLinesOnPath(const lanelet::Ids &) const -> lanelet::ConstLineStrings3d; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 253f8ae4961..037dfeb3baa 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -82,8 +82,6 @@ HdMapUtils::HdMapUtils( routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)); all_graphs.push_back( routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::PEDESTRIAN)); - shoulder_lanelets_ = - lanelet::utils::query::shoulderLanelets(lanelet::utils::query::laneletLayer(lanelet_map_ptr_)); } auto HdMapUtils::getAllCanonicalizedLaneletPoses( @@ -981,18 +979,6 @@ auto HdMapUtils::getLaneletLength(const lanelet::Id lanelet_id) const -> double return ret; } -auto HdMapUtils::getPreviousRoadShoulderLanelet(const lanelet::Id lanelet_id) const -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & shoulder_lanelet : shoulder_lanelets_) { - if (lanelet::geometry::follows(shoulder_lanelet, lanelet)) { - ids.push_back(shoulder_lanelet.id()); - } - } - return ids; -} - auto HdMapUtils::getPreviousLaneletIds( const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids @@ -1040,18 +1026,6 @@ auto HdMapUtils::getPreviousLaneletIds( return sortAndUnique(ids); } -auto HdMapUtils::getNextRoadShoulderLanelet(const lanelet::Id lanelet_id) const -> lanelet::Ids -{ - lanelet::Ids ids; - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lanelet_id); - for (const auto & shoulder_lanelet : shoulder_lanelets_) { - if (lanelet::geometry::follows(lanelet, shoulder_lanelet)) { - ids.push_back(shoulder_lanelet.id()); - } - } - return ids; -} - auto HdMapUtils::getNextLaneletIds( const lanelet::Id lanelet_id, const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids From 1067ce724cd14848f5582d98d96fd151682ac982 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 15:07:02 +0900 Subject: [PATCH 07/12] refactor: add routing graph argument to HdMapUtils::get{Next|Previous}LaneletIds --- .../traffic_simulator/hdmap_utils/hdmap_utils.hpp | 12 ++++++++---- .../src/hdmap_utils/hdmap_utils.cpp | 14 ++++++++------ 2 files changed, 16 insertions(+), 10 deletions(-) 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 90214542b6a..5f8394ed2de 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 @@ -201,8 +201,10 @@ class HdMapUtils const geometry_msgs::msg::Point &, const double distance_threshold, const std::size_t search_count = 5) const -> lanelet::Ids; - auto getNextLaneletIds(const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const -> lanelet::Ids; + auto getNextLaneletIds( + const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const + -> lanelet::Ids; auto getNextLaneletIds( const lanelet::Ids &, const std::string & turn_direction, @@ -219,8 +221,10 @@ class HdMapUtils const traffic_simulator::RoutingGraphType type = traffic_simulator::RoutingGraphType::VEHICLE) const -> lanelet::Ids; - auto getPreviousLaneletIds(const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = - traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const -> lanelet::Ids; + auto getPreviousLaneletIds( + const lanelet::Ids &, const traffic_simulator::RoutingGraphType type = + traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER) const + -> lanelet::Ids; auto getPreviousLaneletIds( const lanelet::Ids &, const std::string & turn_direction, diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 037dfeb3baa..afef7bd343d 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -991,12 +991,13 @@ auto HdMapUtils::getPreviousLaneletIds( return ids; } -/// @todo add RoutingGraphType argument after fixing the bug to get next lanelet instead of previous one -auto HdMapUtils::getPreviousLaneletIds(const lanelet::Ids & lanelet_ids) const -> lanelet::Ids +auto HdMapUtils::getPreviousLaneletIds( + const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { lanelet::Ids ids; for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id); + ids += getNextLaneletIds(id, type); } return sortAndUnique(ids); } @@ -1038,12 +1039,13 @@ auto HdMapUtils::getNextLaneletIds( return ids; } -/// @todo add RoutingGraphType argument after supporting a routing graph with road shoulder lanelets -auto HdMapUtils::getNextLaneletIds(const lanelet::Ids & lanelet_ids) const -> lanelet::Ids +auto HdMapUtils::getNextLaneletIds( + const lanelet::Ids & lanelet_ids, const traffic_simulator::RoutingGraphType type) const + -> lanelet::Ids { lanelet::Ids ids; for (const auto & id : lanelet_ids) { - ids += getNextLaneletIds(id); + ids += getNextLaneletIds(id, type); } return sortAndUnique(ids); } From 97f4347cc68ab51f829d57e31b42adb5ff8ea2ee Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 15:07:48 +0900 Subject: [PATCH 08/12] refactor: GermanRoadShoulderPassableVehicle::canPass --- .../hdmap_utils/traffic_rules.hpp | 43 +++++++++---------- 1 file changed, 21 insertions(+), 22 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp index cdad52fe9ef..7acbbeb5e5b 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp @@ -32,39 +32,38 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV using lanelet::traffic_rules::GermanVehicle::GermanVehicle; protected: + /// @note this function overrides and adds road shoulder handling to GenericTrafficRules::canPass lanelet::Optional canPass( const std::string & type, const std::string & /*location*/) const override { - using ParticipantsMap = std::map>; - using Value = lanelet::AttributeValueString; - using Participants = lanelet::Participants; - const static ParticipantsMap ParticipantMap{ + using lanelet::AttributeValueString; + using lanelet::Participants; + const static std::map> participants_map{ // clang-format off - {"", {Participants::Vehicle}}, - {Value::Road, {Participants::Vehicle, Participants::Bicycle}}, - {"road_shoulder", {Participants::Vehicle, Participants::Bicycle}}, // add road_shoulder - {Value::Highway, {Participants::Vehicle}}, - {Value::BicycleLane, {Participants::Bicycle}}, - {Value::PlayStreet, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, - {Value::EmergencyLane, {Participants::VehicleEmergency}}, - {Value::Exit, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, - {Value::Walkway, {Participants::Pedestrian}}, - {Value::Crosswalk, {Participants::Pedestrian}}, - {Value::Stairs, {Participants::Pedestrian}}, - {Value::SharedWalkway, {Participants::Pedestrian, Participants::Bicycle}} + {"", {Participants::Vehicle}}, + {AttributeValueString::Road, {Participants::Vehicle, Participants::Bicycle}}, + {"road_shoulder", {Participants::Vehicle, Participants::Bicycle}}, // add road_shoulder + {AttributeValueString::Highway, {Participants::Vehicle}}, + {AttributeValueString::BicycleLane, {Participants::Bicycle}}, + {AttributeValueString::PlayStreet, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, + {AttributeValueString::EmergencyLane, {Participants::VehicleEmergency}}, + {AttributeValueString::Exit, {Participants::Pedestrian, Participants::Bicycle, Participants::Vehicle}}, + {AttributeValueString::Walkway, {Participants::Pedestrian}}, + {AttributeValueString::Crosswalk, {Participants::Pedestrian}}, + {AttributeValueString::Stairs, {Participants::Pedestrian}}, + {AttributeValueString::SharedWalkway, {Participants::Pedestrian, Participants::Bicycle}} // clang-format on }; - auto participants = ParticipantMap.find(type); - if (participants == ParticipantMap.end()) { + auto participants = participants_map.find(type); + if (participants == participants_map.end()) { return {}; } - // cspell: ignore startswith - auto startswith = [](const std::string & str, const std::string & substr) { + auto startsWith = [](const std::string & str, const std::string & substr) { return str.compare(0, substr.size(), substr) == 0; }; - return lanelet::utils::anyOf(participants->second, [this, startswith](auto & participant) { - return startswith(this->participant(), participant); + return lanelet::utils::anyOf(participants->second, [this, startsWith](auto & participant) { + return startsWith(this->participant(), participant); }); } }; From 1ab6ef3b512ac47452e2621821f479f0e26414cc Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 16:44:08 +0900 Subject: [PATCH 09/12] refactor: add routing graph argument to HdMapUtils::getRoute --- .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp | 6 ++++-- .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 7 +++---- 2 files changed, 7 insertions(+), 6 deletions(-) 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 5f8394ed2de..78c975bbb44 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 @@ -255,8 +255,10 @@ 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 - -> 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; auto getSpeedLimit(const lanelet::Ids &) const -> double; diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index afef7bd343d..11008c749bb 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -901,12 +901,11 @@ auto HdMapUtils::getFollowingLanelets( } auto HdMapUtils::getRoute( - const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, bool allow_lane_change) const - -> lanelet::Ids + const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, bool allow_lane_change, + const traffic_simulator::RoutingGraphType type) const -> lanelet::Ids { return routing_graphs_->getRoute( - from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, allow_lane_change, - traffic_simulator::RoutingGraphType::VEHICLE); + from_lanelet_id, to_lanelet_id, lanelet_map_ptr_, allow_lane_change, type); } auto HdMapUtils::getCenterPointsSpline(const lanelet::Id lanelet_id) const From 3176302ff40225a4dd6d6012cc6f4c3d00cfb728 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 16:46:11 +0900 Subject: [PATCH 10/12] chore: add routingWithRoadShoulder test --- .../test/src/hdmap_utils/test_hdmap_utils.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 501e64e8754..9d2ba7fa9d8 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 @@ -2735,3 +2735,18 @@ TEST_F(HdMapUtilsTest_StandardMap, getPreviousLanelets) EXPECT_EQ(result_previous, actual_previous); } + +TEST_F(HdMapUtilsTest_WithRoadShoulderMap, routingWithRoadShoulder) +{ + // default: traffic_simulator::RoutingGraphType::VEHICLE + const auto default_route = hdmap_utils.getRoute(34693, 34615, false); + 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); + 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 + EXPECT_EQ(route_with_road_shoulder[2], 34768); // road shoulder + EXPECT_EQ(route_with_road_shoulder[3], 34615); +} \ No newline at end of file From da92b3e65a92aec211311e9d008e2dabe0e5ba80 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 17:47:25 +0900 Subject: [PATCH 11/12] refactor: delete unused code --- simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 11008c749bb..4a66bbe6eb7 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -77,11 +77,6 @@ HdMapUtils::HdMapUtils( THROW_SIMULATION_ERROR("Failed to load lanelet map (", ss.str(), ")"); } overwriteLaneletsCenterline(); - std::vector all_graphs; - all_graphs.push_back( - routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::VEHICLE)); - all_graphs.push_back( - routing_graphs_->routing_graph(traffic_simulator::RoutingGraphType::PEDESTRIAN)); } auto HdMapUtils::getAllCanonicalizedLaneletPoses( From edf7bf9991cd9e3ffb9cd1a5026b7499e4520d86 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 25 Nov 2024 18:03:49 +0900 Subject: [PATCH 12/12] chore: add last empty line to test_hdmap_utils.cpp --- .../traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9d2ba7fa9d8..904eaf66b0f 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 @@ -2749,4 +2749,4 @@ TEST_F(HdMapUtilsTest_WithRoadShoulderMap, routingWithRoadShoulder) EXPECT_EQ(route_with_road_shoulder[1], 34696); // road shoulder EXPECT_EQ(route_with_road_shoulder[2], 34768); // road shoulder EXPECT_EQ(route_with_road_shoulder[3], 34615); -} \ No newline at end of file +}