scenario_simulator_v2 C++ API
hdmap_utils.hpp
Go to the documentation of this file.
1 // Copyright 2015 TIER IV, Inc. All rights reserved.
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 TRAFFIC_SIMULATOR__HDMAP_UTILS__HDMAP_UTILS_HPP_
16 #define TRAFFIC_SIMULATOR__HDMAP_UTILS__HDMAP_UTILS_HPP_
17 
18 #include <geometry_msgs/msg/vector3.h>
19 #include <lanelet2_core/LaneletMap.h>
20 #include <lanelet2_core/geometry/Lanelet.h>
21 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
22 #include <lanelet2_core/primitives/LaneletSequence.h>
23 #include <lanelet2_matching/LaneletMatching.h>
24 #include <lanelet2_routing/Route.h>
25 #include <lanelet2_routing/RoutingCost.h>
26 #include <lanelet2_routing/RoutingGraph.h>
27 #include <lanelet2_routing/RoutingGraphContainer.h>
28 #include <lanelet2_traffic_rules/TrafficRulesFactory.h>
29 #include <tf2/LinearMath/Matrix3x3.h>
30 
31 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
32 #include <autoware_lanelet2_extension/utility/query.hpp>
33 #include <autoware_lanelet2_extension/utility/utilities.hpp>
34 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
35 #include <boost/filesystem.hpp>
36 #include <geographic_msgs/msg/geo_point.hpp>
40 #include <geometry_msgs/msg/pose_stamped.hpp>
41 #include <map>
42 #include <memory>
43 #include <optional>
44 #include <rclcpp/rclcpp.hpp>
45 #include <string>
46 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
51 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
52 #include <traffic_simulator_msgs/msg/entity_status.hpp>
53 #include <tuple>
54 #include <unordered_map>
55 #include <utility>
56 #include <vector>
57 #include <visualization_msgs/msg/marker_array.hpp>
58 
59 namespace hdmap_utils
60 {
61 enum class LaneletType { LANE, CROSSWALK };
62 
64 {
65 public:
66  explicit HdMapUtils(const boost::filesystem::path &, const geographic_msgs::msg::GeoPoint &);
67 
68  auto canChangeLane(
69  const lanelet::Id from, const lanelet::Id to,
71  traffic_simulator::RoutingConfiguration().routing_graph_type) const -> bool;
72 
74  const lanelet::Id, const double s, const lanelet::Ids &,
75  const double forward_distance = 20) const -> std::vector<geometry_msgs::msg::Point>;
76 
77  auto countLaneChanges(
80  const traffic_simulator::RoutingConfiguration & routing_configuration =
81  traffic_simulator::RoutingConfiguration()) const -> std::optional<std::pair<int, int>>;
82 
83  auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids;
84 
85  auto generateMarker() const -> visualization_msgs::msg::MarkerArray;
86 
87  auto getCenterPoints(const lanelet::Ids &) const -> std::vector<geometry_msgs::msg::Point>;
88 
89  auto getCenterPoints(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
90 
91  auto getCenterPointsSpline(const lanelet::Id) const
92  -> std::shared_ptr<math::geometry::CatmullRomSpline>;
93 
95  const geometry_msgs::msg::Pose &, const double distance_thresh = 30.0,
96  const bool include_crosswalk = false) const -> std::optional<lanelet::Id>;
97 
99  const lanelet::Id lanelet_id, const lanelet::Id crossing_lanelet_id) const
100  -> std::optional<double>;
101 
102  auto getConflictingCrosswalkIds(const lanelet::Ids &) const -> lanelet::Ids;
103 
105  const lanelet::Ids &, const traffic_simulator::RoutingGraphType type =
106  traffic_simulator::RoutingConfiguration().routing_graph_type) const
107  -> lanelet::Ids;
108 
110  const lanelet::Ids & route_lanelets,
111  const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>;
112 
114  const lanelet::Ids & route_lanelets,
115  const std::vector<geometry_msgs::msg::Point> & waypoints) const -> std::optional<double>;
116 
119  const lanelet::Id traffic_light_id) const -> std::optional<double>;
120 
122  const std::vector<geometry_msgs::msg::Point> & waypoints,
123  const lanelet::Id traffic_light_id) const -> std::optional<double>;
124 
126  const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon = 100,
127  const bool include_current_lanelet_id = true,
129  traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids;
130 
132  const lanelet::Id, const double distance = 100, const bool include_self = true,
134  traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids;
135 
136  auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double;
137 
139  const geometry_msgs::msg::Pose & from,
140  const traffic_simulator::lane_change::Parameter & lane_change_parameter,
141  const double maximum_curvature_threshold, const double target_trajectory_length,
142  const double forward_distance_threshold) const
143  -> std::optional<std::pair<math::geometry::HermiteCurve, double>>;
144 
147  const traffic_simulator::lane_change::Parameter & lane_change_parameter) const
148  -> std::optional<std::pair<math::geometry::HermiteCurve, double>>;
149 
151  const lanelet::Id, const traffic_simulator::lane_change::Direction,
153  traffic_simulator::RoutingConfiguration().routing_graph_type) const
154  -> std::optional<lanelet::Id>;
155 
157  const lanelet::Id, const traffic_simulator::lane_change::Direction, const std::uint8_t shift,
159  traffic_simulator::RoutingConfiguration().routing_graph_type) const
160  -> std::optional<lanelet::Id>;
161 
162  auto getLaneletIds() const -> lanelet::Ids;
163 
164  auto getLaneletPolygon(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
165 
166  auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets;
167 
168  auto getNearbyLaneletIds(
169  const geometry_msgs::msg::Point &, const double distance_threshold,
170  const bool include_crosswalk, const std::size_t search_count = 5) const -> lanelet::Ids;
171 
172  auto getNearbyLaneletIds(
173  const geometry_msgs::msg::Point &, const double distance_threshold,
174  const std::size_t search_count = 5) const -> lanelet::Ids;
175 
176  auto getPreviousLanelets(
177  const lanelet::Id, const double backward_horizon = 100,
179  traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids;
180 
181  auto getRightOfWayLaneletIds(const lanelet::Ids &) const
182  -> std::unordered_map<lanelet::Id, lanelet::Ids>;
183 
184  auto getRightOfWayLaneletIds(const lanelet::Id) const -> lanelet::Ids;
185 
186  auto getRoute(
187  const lanelet::Id from, const lanelet::Id to,
188  const traffic_simulator::RoutingConfiguration & routing_configuration =
189  traffic_simulator::RoutingConfiguration()) const -> lanelet::Ids;
190 
191  auto getSpeedLimit(
192  const lanelet::Ids &, const traffic_simulator::RoutingGraphType type =
193  traffic_simulator::RoutingConfiguration().routing_graph_type) const
194  -> double;
195 
196  auto getTangentVector(const lanelet::Id, const double s) const
197  -> std::optional<geometry_msgs::msg::Vector3>;
198 
200  const lanelet::Id traffic_light_id, const std::string &,
201  const bool allow_infer_position = false) const -> std::optional<geometry_msgs::msg::Point>;
202 
203  auto getTrafficLightIds() const -> lanelet::Ids;
204 
205  auto getTrafficLightIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids;
206 
207  auto getTrafficLightRegulatoryElement(const lanelet::Id) const -> lanelet::TrafficLight::Ptr;
208 
209  auto getTrafficLightRegulatoryElementIDsFromTrafficLight(const lanelet::Id) const -> lanelet::Ids;
210 
211  auto getTrafficLightStopLineIds(const lanelet::Id traffic_light_id) const -> lanelet::Ids;
212 
213  auto getTrafficLightStopLinesPoints(const lanelet::Id traffic_light_id) const
214  -> std::vector<std::vector<geometry_msgs::msg::Point>>;
215 
216  auto insertMarkerArray(
217  visualization_msgs::msg::MarkerArray &, const visualization_msgs::msg::MarkerArray &) const
218  -> void;
219 
220  auto isInIntersection(const lanelet::Id) const -> bool;
221 
222  auto isInLanelet(const lanelet::Id, const double s) const -> bool;
223 
224  auto isInRoute(const lanelet::Id, const lanelet::Ids & route) const -> bool;
225 
226  auto isTrafficLight(const lanelet::Id) const -> bool;
227 
228  auto isTrafficLightRegulatoryElement(const lanelet::Id) const -> bool;
229 
230 private:
233  constexpr static double DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO = 0.8;
234 
235 public:
236  auto toMapBin() const -> autoware_map_msgs::msg::LaneletMapBin;
237 
238  auto toMapPoints(const lanelet::Id, const std::vector<double> & s) const
239  -> std::vector<geometry_msgs::msg::Point>;
240 
241 private:
245  // @{
246  mutable CenterPointsCache center_points_cache_;
247  mutable LaneletLengthCache lanelet_length_cache_;
248  // @}
249 
250  lanelet::LaneletMapPtr lanelet_map_ptr_;
251 
252  class RoutingGraphs
253  {
254  public:
255  explicit RoutingGraphs(const lanelet::LaneletMapPtr & lanelet_map);
256 
258  {
259  lanelet::traffic_rules::TrafficRulesPtr rules;
260  lanelet::routing::RoutingGraphPtr graph;
262  };
263 
264  [[nodiscard]] lanelet::routing::RoutingGraphPtr routing_graph(
265  const traffic_simulator::RoutingGraphType type) const;
266 
267  [[nodiscard]] lanelet::traffic_rules::TrafficRulesPtr traffic_rule(
268  const traffic_simulator::RoutingGraphType type) const;
269 
270  [[nodiscard]] auto getRoute(
271  const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id,
272  lanelet::LaneletMapPtr lanelet_map_ptr,
273  const traffic_simulator::RoutingConfiguration & routing_configuration =
274  traffic_simulator::RoutingConfiguration()) -> lanelet::Ids;
275 
276  private:
277  [[nodiscard]] RouteCache & route_cache(const traffic_simulator::RoutingGraphType type);
278 
279  RuleWithGraph vehicle;
280 
281  RuleWithGraph vehicle_with_road_shoulder;
282 
283  RuleWithGraph pedestrian;
284  };
285 
286  std::unique_ptr<RoutingGraphs> routing_graphs_;
287 
288  template <typename Lanelet>
289  auto getLaneletIds(const std::vector<Lanelet> & lanelets) const -> lanelet::Ids
290  {
291  lanelet::Ids ids;
292  std::transform(
293  lanelets.begin(), lanelets.end(), std::back_inserter(ids),
294  [](const auto & lanelet) { return lanelet.id(); });
295  return ids;
296  }
297 
298  auto absoluteHull(
299  const lanelet::BasicPolygon2d & relative_hull, const lanelet::matching::Pose2d &) const
300  -> lanelet::BasicPolygon2d;
301 
302  auto calcEuclidDist(
303  const std::vector<double> & x, const std::vector<double> & y,
304  const std::vector<double> & z) const -> std::vector<double>;
305 
306  auto calculateAccumulatedLengths(const lanelet::ConstLineString3d &) const -> std::vector<double>;
307 
308  auto calculateSegmentDistances(const lanelet::ConstLineString3d &) const -> std::vector<double>;
309 
310  auto excludeSubtypeLanelets(
311  const std::vector<std::pair<double, lanelet::Lanelet>> &, const char subtype[]) const
312  -> std::vector<std::pair<double, lanelet::Lanelet>>;
313 
314  auto filterLanelets(const lanelet::Lanelets &, const char subtype[]) const -> lanelet::Lanelets;
315 
316  auto findNearestIndexPair(
317  const std::vector<double> & accumulated_lengths, const double target_length) const
318  -> std::pair<std::size_t, std::size_t>;
319 
320  auto generateFineCenterline(const lanelet::ConstLanelet &, const double resolution) const
321  -> lanelet::LineString3d;
322 
326  const double tangent_vector_size = 100) const -> math::geometry::HermiteCurve;
327 
328  auto getTrafficLightRegulatoryElementsOnPath(const lanelet::Ids &) const
329  -> std::vector<std::shared_ptr<const lanelet::autoware::AutowareTrafficLight>>;
330 
331  auto getTrafficLights(const lanelet::Id traffic_light_id) const
332  -> std::vector<lanelet::AutowareTrafficLightConstPtr>;
333 
334  auto getTrafficSignRegulatoryElementsOnPath(const lanelet::Ids &) const
335  -> std::vector<std::shared_ptr<const lanelet::TrafficSign>>;
336 
337  auto getTrafficSignRegulatoryElements() const
338  -> std::vector<std::shared_ptr<const lanelet::TrafficSign>>;
339 
340  auto getVectorFromPose(const geometry_msgs::msg::Pose &, const double magnitude) const
341  -> geometry_msgs::msg::Vector3;
342 
343  auto mapCallback(const autoware_map_msgs::msg::LaneletMapBin &) const -> void;
344 
345  auto resamplePoints(const lanelet::ConstLineString3d &, const std::int32_t num_segments) const
346  -> lanelet::BasicPoints3d;
347 
348  auto toPoint2d(const geometry_msgs::msg::Point &) const -> lanelet::BasicPoint2d;
349 
350  auto toPolygon(const lanelet::ConstLineString3d &) const
351  -> std::vector<geometry_msgs::msg::Point>;
352 };
353 } // namespace hdmap_utils
354 
355 #endif // TRAFFIC_SIMULATOR__HDMAP_UTILS__HDMAP_UTILS_HPP_
Definition: cache.hpp:67
Definition: hdmap_utils.hpp:64
auto getTangentVector(const lanelet::Id, const double s) const -> std::optional< geometry_msgs::msg::Vector3 >
Definition: hdmap_utils.cpp:927
auto getRoute(const lanelet::Id from, const lanelet::Id to, const traffic_simulator::RoutingConfiguration &routing_configuration=traffic_simulator::RoutingConfiguration()) const -> lanelet::Ids
Definition: hdmap_utils.cpp:604
auto toMapPoints(const lanelet::Id, const std::vector< double > &s) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:916
HdMapUtils(const boost::filesystem::path &, const geographic_msgs::msg::GeoPoint &)
Definition: hdmap_utils.cpp:65
auto isInIntersection(const lanelet::Id) const -> bool
Definition: hdmap_utils.cpp:906
auto getCenterPointsSpline(const lanelet::Id) const -> std::shared_ptr< math::geometry::CatmullRomSpline >
Definition: hdmap_utils.cpp:612
auto getDistanceToTrafficLightStopLine(const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: hdmap_utils.cpp:1212
auto getTrafficLightStopLinesPoints(const lanelet::Id traffic_light_id) const -> std::vector< std::vector< geometry_msgs::msg::Point >>
Definition: hdmap_utils.cpp:1154
auto getCollisionPointInLaneCoordinate(const lanelet::Id lanelet_id, const lanelet::Id crossing_lanelet_id) const -> std::optional< double >
Definition: hdmap_utils.cpp:210
auto getTrafficLightIds() const -> lanelet::Ids
Definition: hdmap_utils.cpp:672
auto getSpeedLimit(const lanelet::Ids &, const traffic_simulator::RoutingGraphType type=traffic_simulator::RoutingConfiguration().routing_graph_type) const -> double
Definition: hdmap_utils.cpp:427
auto getConflictingLaneIds(const lanelet::Ids &, const traffic_simulator::RoutingGraphType type=traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids
Definition: hdmap_utils.cpp:259
auto isInRoute(const lanelet::Id, const lanelet::Ids &route) const -> bool
Definition: hdmap_utils.cpp:517
auto canChangeLane(const lanelet::Id from, const lanelet::Id to, const traffic_simulator::RoutingGraphType type=traffic_simulator::RoutingConfiguration().routing_graph_type) const -> bool
Definition: hdmap_utils.cpp:933
auto getRightOfWayLaneletIds(const lanelet::Ids &) const -> std::unordered_map< lanelet::Id, lanelet::Ids >
Definition: hdmap_utils.cpp:1053
auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids
Definition: hdmap_utils.cpp:131
auto getLaneChangeTrajectory(const geometry_msgs::msg::Pose &from, const traffic_simulator::lane_change::Parameter &lane_change_parameter, const double maximum_curvature_threshold, const double target_trajectory_length, const double forward_distance_threshold) const -> std::optional< std::pair< math::geometry::HermiteCurve, double >>
Definition: hdmap_utils.cpp:814
auto getTrafficLightRegulatoryElement(const lanelet::Id) const -> lanelet::TrafficLight::Ptr
Definition: hdmap_utils.cpp:1403
auto getAltitude(const traffic_simulator_msgs::msg::LaneletPose &) const -> double
Definition: hdmap_utils.cpp:204
auto isTrafficLightRegulatoryElement(const lanelet::Id) const -> bool
Definition: hdmap_utils.cpp:1396
auto clipTrajectoryFromLaneletIds(const lanelet::Id, const double s, const lanelet::Ids &, const double forward_distance=20) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:297
auto getTrafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string &, const bool allow_infer_position=false) const -> std::optional< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:691
auto getTrafficLightRegulatoryElementIDsFromTrafficLight(const lanelet::Id) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1411
auto getTrafficLightIdsOnPath(const lanelet::Ids &route_lanelets) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1176
auto isTrafficLight(const lanelet::Id) const -> bool
Definition: hdmap_utils.cpp:1382
auto getLaneletIds() const -> lanelet::Ids
Definition: hdmap_utils.cpp:106
auto getFollowingLanelets(const lanelet::Id current_lanelet_id, const lanelet::Ids &route, const double horizon=100, const bool include_current_lanelet_id=true, const traffic_simulator::RoutingGraphType type=traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids
Definition: hdmap_utils.cpp:522
auto generateMarker() const -> visualization_msgs::msg::MarkerArray
Definition: hdmap_utils.cpp:965
auto getClosestLaneletId(const geometry_msgs::msg::Pose &, const double distance_thresh=30.0, const bool include_crosswalk=false) const -> std::optional< lanelet::Id >
Definition: hdmap_utils.cpp:395
auto getTrafficLightStopLineIds(const lanelet::Id traffic_light_id) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1142
auto getCenterPoints(const lanelet::Ids &) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:619
auto getLaneletPolygon(const lanelet::Id) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:115
auto toMapBin() const -> autoware_map_msgs::msg::LaneletMapBin
Definition: hdmap_utils.cpp:942
auto getLaneChangeableLaneletId(const lanelet::Id, const traffic_simulator::lane_change::Direction, const traffic_simulator::RoutingGraphType type=traffic_simulator::RoutingConfiguration().routing_graph_type) const -> std::optional< lanelet::Id >
Definition: hdmap_utils.cpp:467
auto getPreviousLanelets(const lanelet::Id, const double backward_horizon=100, const traffic_simulator::RoutingGraphType type=traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids
Definition: hdmap_utils.cpp:491
auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets
Definition: hdmap_utils.cpp:1373
auto isInLanelet(const lanelet::Id, const double s) const -> bool
Definition: hdmap_utils.cpp:911
auto getConflictingCrosswalkIds(const lanelet::Ids &) const -> lanelet::Ids
Definition: hdmap_utils.cpp:275
auto getNearbyLaneletIds(const geometry_msgs::msg::Point &, const double distance_threshold, const bool include_crosswalk, const std::size_t search_count=5) const -> lanelet::Ids
Definition: hdmap_utils.cpp:166
auto insertMarkerArray(visualization_msgs::msg::MarkerArray &, const visualization_msgs::msg::MarkerArray &) const -> void
Definition: hdmap_utils.cpp:958
auto countLaneChanges(const traffic_simulator_msgs::msg::LaneletPose &from, const traffic_simulator_msgs::msg::LaneletPose &to, const traffic_simulator::RoutingConfiguration &routing_configuration=traffic_simulator::RoutingConfiguration()) const -> std::optional< std::pair< int, int >>
Definition: hdmap_utils.cpp:72
Definition: cache.hpp:109
Definition: cache.hpp:30
Definition: catmull_rom_spline_interface.hpp:30
Definition: sim_model_delay_steer_acc_geared_wo_fall_guard.hpp:26
Definition: cache.hpp:28
LaneletType
Definition: hdmap_utils.hpp:61
Definition: bounding_box.hpp:32
auto distance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
Definition: detection_sensor.cpp:42
Definition: lanelet_wrapper.hpp:40
TrajectoryShape
Definition: lane_change.hpp:31
Direction
Definition: lane_change.hpp:29
auto route(const lanelet::Id from_lanelet_id, const lanelet::Id to_lanelet_id, const RoutingConfiguration &routing_configuration=RoutingConfiguration()) -> lanelet::Ids
Definition: route.cpp:33
geometry_msgs::msg::Pose Pose
Definition: lanelet_wrapper.hpp:66
geometry_msgs::msg::Point Point
Definition: lanelet_wrapper.hpp:65
geometry_msgs::msg::Vector3 Vector3
Definition: lanelet_wrapper.hpp:69
RoutingGraphType
Definition: routing_graph_type.hpp:24
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:23
std::string string
Definition: junit5.hpp:26
lanelet::traffic_rules::TrafficRulesPtr rules
Definition: hdmap_utils.hpp:259
RouteCache route_cache
Definition: hdmap_utils.hpp:261
lanelet::routing::RoutingGraphPtr graph
Definition: hdmap_utils.hpp:260
Definition: routing_configuration.hpp:24
parameters for behavior plugin
Definition: lane_change.hpp:75