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/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..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 @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -200,28 +201,40 @@ 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, 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, 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, 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, @@ -242,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; @@ -337,8 +352,6 @@ class HdMapUtils lanelet::LaneletMapPtr lanelet_map_ptr_; - lanelet::ConstLanelets shoulder_lanelets_; - class RoutingGraphs { public: @@ -367,6 +380,8 @@ class HdMapUtils RuleWithGraph vehicle; + RuleWithGraph vehicle_with_road_shoulder; + RuleWithGraph pedestrian; }; @@ -412,10 +427,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/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..7acbbeb5e5b --- /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: + /// @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 lanelet::AttributeValueString; + using lanelet::Participants; + const static std::map> participants_map{ + // clang-format off + {"", {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 = participants_map.find(type); + if (participants == participants_map.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/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 002b69dde3c..4a66bbe6eb7 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -77,13 +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)); - shoulder_lanelets_ = - lanelet::utils::query::shoulderLanelets(lanelet::utils::query::laneletLayer(lanelet_map_ptr_)); } auto HdMapUtils::getAllCanonicalizedLaneletPoses( @@ -903,12 +896,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 @@ -981,40 +973,25 @@ 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; -} - -/// @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; } -/// @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); } @@ -1044,40 +1021,25 @@ 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; -} - -/// @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; } -/// @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); } @@ -2199,6 +2161,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 +2176,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 +2193,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 +2209,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: 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); 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..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 @@ -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); +}