|
| 1 | +// Copyright 2025 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#ifndef AUTOWARE_LANELET2_UTILS__TOPOLOGY_HPP_ |
| 16 | +#define AUTOWARE_LANELET2_UTILS__TOPOLOGY_HPP_ |
| 17 | + |
| 18 | +#include <lanelet2_core/primitives/Lanelet.h> |
| 19 | +#include <lanelet2_routing/Forward.h> |
| 20 | +#include <lanelet2_traffic_rules/TrafficRulesFactory.h> |
| 21 | + |
| 22 | +#include <optional> |
| 23 | +#include <vector> |
| 24 | + |
| 25 | +namespace autoware::lanelet2_utils |
| 26 | +{ |
| 27 | +/** |
| 28 | + * @brief instantiate RoutingGraph from given LaneletMap only from "road" lanes |
| 29 | + * @param location [in, opt, lanelet::Locations::Germany] location value |
| 30 | + * @param participant [in, opt, lanelet::Participants::Vehicle] participant value |
| 31 | + * @return RoutingGraph object without road_shoulder and bicycle_lane |
| 32 | + */ |
| 33 | +lanelet::routing::RoutingGraphConstPtr instantiate_routing_graph( |
| 34 | + lanelet::LaneletMapConstPtr lanelet_map, const char * location = lanelet::Locations::Germany, |
| 35 | + const char * participant = lanelet::Participants::Vehicle); |
| 36 | + |
| 37 | +/** |
| 38 | + * @brief get the left adjacent and same_direction lanelet on the routing graph if exists regardless |
| 39 | + * of lane change permission |
| 40 | + * @param [in] lanelet input lanelet |
| 41 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 42 | + * @return optional of left adjacent lanelet(nullopt if there is no such adjacent lanelet) |
| 43 | + */ |
| 44 | +std::optional<lanelet::ConstLanelet> left_lanelet( |
| 45 | + const lanelet::ConstLanelet & lanelet, |
| 46 | + const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 47 | + |
| 48 | +/** |
| 49 | + * @brief get the right adjacent and same_direction lanelet on the routing graph if exists |
| 50 | + * @param [in] lanelet input lanelet |
| 51 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 52 | + * @return optional of right adjacent lanelet(nullopt if there is no such adjacent lanelet) |
| 53 | + */ |
| 54 | +std::optional<lanelet::ConstLanelet> right_lanelet( |
| 55 | + const lanelet::ConstLanelet & lanelet, |
| 56 | + const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 57 | + |
| 58 | +/** |
| 59 | + * @brief get left_lanelet() or sibling lanelet. If `lanelet` has turn_direction, search for sibling |
| 60 | + * lanelet is limited to the one with same turn_direction value |
| 61 | + * @param [in] lanelet input lanelet |
| 62 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 63 | + * @return optional of aforementioned lanelet(nullopt if there is no such lanelet) |
| 64 | + */ |
| 65 | +/* |
| 66 | +std::optional<lanelet::ConstLanelet> left_similar_lanelet( |
| 67 | +const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, |
| 68 | +const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 69 | +*/ |
| 70 | + |
| 71 | +/** |
| 72 | + * @brief get right_lanelet() or sibling lanelet. If `lanelet` has turn_direction, search for |
| 73 | + * sibling lanelet is limited to the one with same turn_direction value |
| 74 | + * @param [in] lanelet input lanelet |
| 75 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 76 | + * @return optional of aforementioned lanelet(nullopt if there is no such lanelet) |
| 77 | + */ |
| 78 | +/* |
| 79 | +std::optional<lanelet::ConstLanelet> right_similar_lanelet( |
| 80 | +const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, |
| 81 | +const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 82 | +*/ |
| 83 | + |
| 84 | +/** |
| 85 | + * @brief get the left adjacent and opposite_direction lanelet on the routing graph if exists |
| 86 | + * @param [in] lanelet input lanelet |
| 87 | + * @param [in] lanelet_map lanelet_map containing `lanelet` |
| 88 | + * @return optional of the left opposite lanelet(nullopt if there is not such opposite lanelet) |
| 89 | + */ |
| 90 | +std::optional<lanelet::ConstLanelet> left_opposite_lanelet( |
| 91 | + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map); |
| 92 | + |
| 93 | +/** |
| 94 | + * @brief get the right adjacent and opposite_direction lanelet on the routing graph if exists |
| 95 | + * @param [in] lanelet input lanelet |
| 96 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 97 | + * @return optional of the right opposite lanelet(nullopt if there is no such opposite lanelet) |
| 98 | + */ |
| 99 | +std::optional<lanelet::ConstLanelet> right_opposite_lanelet( |
| 100 | + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map); |
| 101 | + |
| 102 | +/** |
| 103 | + * @brief get the leftmost same_direction lanelet if exists |
| 104 | + * @param [in] lanelet input lanelet |
| 105 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 106 | + * @return optional of such lanelet(nullopt if there is no such adjacent lanelet) |
| 107 | + */ |
| 108 | +std::optional<lanelet::ConstLanelet> leftmost_lanelet( |
| 109 | + const lanelet::ConstLanelet & lanelet, |
| 110 | + const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 111 | + |
| 112 | +std::optional<lanelet::ConstLanelet> rightmost_lanelet( |
| 113 | + const lanelet::ConstLanelet & lanelet, |
| 114 | + const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 115 | + |
| 116 | +/** |
| 117 | + * @brief get the left lanelets which are adjacent to `lanelet` |
| 118 | + * @param [in] lanelet input lanelet |
| 119 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 120 | + * @param [in] include_opposite flag if opposite_direction lanelet is included |
| 121 | + * @param [in] invert_opposite_lanelet flag if the opposite lanelet in the output is `.inverted()` |
| 122 | + * or not |
| 123 | + * @return the list of lanelets excluding `lanelet` which is ordered in the *hopping* number from |
| 124 | + * `lanelet` |
| 125 | + */ |
| 126 | +lanelet::ConstLanelets left_lanelets( |
| 127 | + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, |
| 128 | + const lanelet::routing::RoutingGraphConstPtr routing_graph, const bool include_opposite = false, |
| 129 | + const bool invert_opposite_lane = false); |
| 130 | + |
| 131 | +/** |
| 132 | + * @brief get the right lanelets which are adjacent to `lanelet` |
| 133 | + * @param [in] lanelet input lanelet |
| 134 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 135 | + * @param [in] include_opposite flag if opposite_direction lanelet is included |
| 136 | + * @param [in] invert_opposite_lanelet flag if the opposite lanelet in the output is `.inverted()` |
| 137 | + * or not |
| 138 | + * @return the list of lanelets excluding `lanelet` which is ordered in the *hopping* number from |
| 139 | + * `lanelet` |
| 140 | + */ |
| 141 | +lanelet::ConstLanelets right_lanelets( |
| 142 | + const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr lanelet_map, |
| 143 | + const lanelet::routing::RoutingGraphConstPtr routing_graph, const bool include_opposite = false, |
| 144 | + const bool invert_opposite_lane = false); |
| 145 | + |
| 146 | +/** |
| 147 | + * @brief get the following lanelets |
| 148 | + * @param [in] lanelet input lanelet |
| 149 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 150 | + * @return the following lanelets |
| 151 | + */ |
| 152 | +lanelet::ConstLanelets following_lanelets( |
| 153 | + const lanelet::ConstLanelet & lanelet, |
| 154 | + const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 155 | + |
| 156 | +/** |
| 157 | + * @brief get the previous lanelets |
| 158 | + * @param [in] lanelet input lanelet |
| 159 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 160 | + * @return the previous lanelets |
| 161 | + */ |
| 162 | +lanelet::ConstLanelets previous_lanelets( |
| 163 | + const lanelet::ConstLanelet & lanelet, |
| 164 | + const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 165 | + |
| 166 | +/** |
| 167 | + * @brief get the sibling lanelets |
| 168 | + * @param [in] lanelet input lanelet |
| 169 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 170 | + * @return the sibling lanelets excluding `lanelet` |
| 171 | + */ |
| 172 | +lanelet::ConstLanelets sibling_lanelets( |
| 173 | + const lanelet::ConstLanelet & lanelet, |
| 174 | + const lanelet::routing::RoutingGraphConstPtr routing_graph); |
| 175 | + |
| 176 | +/** |
| 177 | + * @brief get Lanelet instances of the designated ids |
| 178 | + * @param [in] lanelet input lanelet |
| 179 | + * @param [in] routing_graph routing_graph containing `lanelet` |
| 180 | + * @return the list of Lanelets in the same order as `ids` |
| 181 | + */ |
| 182 | +lanelet::ConstLanelets from_ids( |
| 183 | + const lanelet::LaneletMapConstPtr lanelet_map, const std::vector<lanelet::Id> & ids); |
| 184 | +} // namespace autoware::lanelet2_utils |
| 185 | + |
| 186 | +#endif // AUTOWARE_LANELET2_UTILS__TOPOLOGY_HPP_ |
0 commit comments