15 #ifndef TRAFFIC_SIMULATOR__HDMAP_UTILS__HDMAP_UTILS_HPP_
16 #define TRAFFIC_SIMULATOR__HDMAP_UTILS__HDMAP_UTILS_HPP_
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>
31 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
32 #include <boost/filesystem.hpp>
33 #include <geographic_msgs/msg/geo_point.hpp>
37 #include <geometry_msgs/msg/pose_stamped.hpp>
38 #include <lanelet2_extension/utility/message_conversion.hpp>
39 #include <lanelet2_extension/utility/query.hpp>
40 #include <lanelet2_extension/utility/utilities.hpp>
44 #include <rclcpp/rclcpp.hpp>
46 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
49 #include <traffic_simulator_msgs/msg/bounding_box.hpp>
50 #include <traffic_simulator_msgs/msg/entity_status.hpp>
52 #include <unordered_map>
55 #include <visualization_msgs/msg/marker_array.hpp>
64 explicit HdMapUtils(
const boost::filesystem::path &,
const geographic_msgs::msg::GeoPoint &);
66 auto canChangeLane(
const lanelet::Id from,
const lanelet::Id to)
const -> bool;
70 std::optional<traffic_simulator_msgs::msg::LaneletPose>, std::optional<lanelet::Id>>;
75 std::optional<traffic_simulator_msgs::msg::LaneletPose>, std::optional<lanelet::Id>>;
78 const lanelet::Id,
const double s,
const lanelet::Ids &,
79 const double forward_distance = 20)
const -> std::vector<geometry_msgs::msg::Point>;
81 auto filterLaneletIds(
const lanelet::Ids &,
const char subtype[])
const -> lanelet::Ids;
83 auto generateMarker()
const -> visualization_msgs::msg::MarkerArray;
86 -> std::vector<traffic_simulator_msgs::msg::LaneletPose>;
92 auto getCenterPoints(
const lanelet::Ids &)
const -> std::vector<geometry_msgs::msg::Point>;
94 auto getCenterPoints(
const lanelet::Id)
const -> std::vector<geometry_msgs::msg::Point>;
97 -> std::shared_ptr<math::geometry::CatmullRomSpline>;
100 const geometry_msgs::msg::Pose &,
const double distance_thresh = 30.0,
101 const bool include_crosswalk =
false)
const -> std::optional<lanelet::Id>;
104 const lanelet::Id lanelet_id,
const lanelet::Id crossing_lanelet_id)
const
105 -> std::optional<double>;
112 const lanelet::Ids & route_lanelets,
116 const lanelet::Ids & route_lanelets,
117 const std::vector<geometry_msgs::msg::Point> & waypoints)
const -> std::optional<double>;
120 const lanelet::Ids & route_lanelets,
124 const lanelet::Ids & route_lanelets,
125 const std::vector<geometry_msgs::msg::Point> & waypoints)
const -> std::optional<double>;
129 const lanelet::Id traffic_light_id)
const -> std::optional<double>;
132 const std::vector<geometry_msgs::msg::Point> & waypoints,
133 const lanelet::Id traffic_light_id)
const -> std::optional<double>;
136 const lanelet::Id lanelet_id,
const lanelet::Ids & candidate_lanelet_ids,
137 const double distance = 100,
const bool include_self =
true)
const -> lanelet::Ids;
140 const lanelet::Id,
const double distance = 100,
const bool include_self =
true)
const
146 const geometry_msgs::msg::Pose & from,
148 const double maximum_curvature_threshold,
const double target_trajectory_length,
149 const double forward_distance_threshold)
const
150 -> std::optional<std::pair<math::geometry::HermiteCurve, double>>;
155 -> std::optional<std::pair<math::geometry::HermiteCurve, double>>;
159 -> std::optional<lanelet::Id>;
163 const std::uint8_t shift)
const -> std::optional<lanelet::Id>;
169 auto getLaneletPolygon(
const lanelet::Id)
const -> std::vector<geometry_msgs::msg::Point>;
171 auto getLanelets(
const lanelet::Ids &)
const -> lanelet::Lanelets;
176 -> std::optional<double>;
178 auto getLeftBound(
const lanelet::Id)
const -> std::vector<geometry_msgs::msg::Point>;
181 const lanelet::Id,
const traffic_simulator_msgs::msg::EntityType &,
182 const bool include_opposite_direction =
true)
const -> lanelet::Ids;
187 -> std::optional<double>;
190 const geometry_msgs::msg::Point &,
const double distance_threshold,
191 const bool include_crosswalk,
const std::size_t search_count = 5)
const -> lanelet::Ids;
194 const geometry_msgs::msg::Point &,
const double distance_threshold,
195 const std::size_t search_count = 5)
const -> lanelet::Ids;
219 auto getRightBound(
const lanelet::Id)
const -> std::vector<geometry_msgs::msg::Point>;
222 lanelet::Id, traffic_simulator_msgs::msg::EntityType,
223 bool include_opposite_direction =
true)
const -> lanelet::Ids;
226 -> std::unordered_map<lanelet::Id, lanelet::Ids>;
230 auto getRoute(
const lanelet::Id from,
const lanelet::Id to,
bool allow_lane_change =
false)
const
239 auto getStopLinePolygon(
const lanelet::Id)
const -> std::vector<geometry_msgs::msg::Point>;
242 -> std::optional<geometry_msgs::msg::Vector3>;
245 -> std::optional<geometry_msgs::msg::Point>;
258 -> std::vector<std::vector<geometry_msgs::msg::Point>>;
261 visualization_msgs::msg::MarkerArray &,
const visualization_msgs::msg::MarkerArray &)
const
264 auto isInLanelet(
const lanelet::Id,
const double s)
const -> bool;
266 auto isInRoute(
const lanelet::Id,
const lanelet::Ids & route)
const -> bool;
273 const geometry_msgs::msg::Pose &,
const traffic_simulator_msgs::msg::BoundingBox &,
274 const bool include_crosswalk,
const double matching_distance = 1.0,
275 const double reduction_ratio = 0.8)
const -> std::optional<lanelet::Id>;
278 const geometry_msgs::msg::Pose &,
const bool include_crosswalk,
279 const double matching_distance = 1.0)
const
280 -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
283 const geometry_msgs::msg::Pose &,
const lanelet::Ids &,
284 const double matching_distance = 1.0)
const
285 -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
288 const geometry_msgs::msg::Point &,
const traffic_simulator_msgs::msg::BoundingBox &,
289 const bool include_crosswalk,
const double matching_distance = 1.0)
const
290 -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
293 const geometry_msgs::msg::Pose &,
const traffic_simulator_msgs::msg::BoundingBox &,
294 const bool include_crosswalk,
const double matching_distance = 1.0)
const
295 -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
298 const geometry_msgs::msg::Pose &,
const lanelet::Id,
const double matching_distance = 1.0)
const
299 -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
302 const geometry_msgs::msg::Pose &,
const lanelet::Id,
const double matching_distance = 5.0,
303 const bool include_opposite_direction =
true)
const
304 -> std::vector<traffic_simulator_msgs::msg::LaneletPose>;
306 auto toMapBin()
const -> autoware_auto_mapping_msgs::msg::HADMapBin;
308 auto toMapPoints(
const lanelet::Id,
const std::vector<double> & s)
const
309 -> std::vector<geometry_msgs::msg::Point>;
312 const -> geometry_msgs::msg::PoseStamped;
324 lanelet::LaneletMapPtr lanelet_map_ptr_;
325 lanelet::routing::RoutingGraphConstPtr vehicle_routing_graph_ptr_;
326 lanelet::traffic_rules::TrafficRulesPtr traffic_rules_vehicle_ptr_;
327 lanelet::routing::RoutingGraphConstPtr pedestrian_routing_graph_ptr_;
328 lanelet::traffic_rules::TrafficRulesPtr traffic_rules_pedestrian_ptr_;
329 lanelet::ConstLanelets shoulder_lanelets_;
331 template <
typename Lanelet>
332 auto getLaneletIds(
const std::vector<Lanelet> & lanelets)
const -> lanelet::Ids
336 lanelets.begin(), lanelets.end(), std::back_inserter(ids),
337 [](
const auto & lanelet) { return lanelet.id(); });
342 const lanelet::BasicPolygon2d & relative_hull,
const lanelet::matching::Pose2d &)
const
343 -> lanelet::BasicPolygon2d;
346 const std::vector<double> & x,
const std::vector<double> & y,
347 const std::vector<double> & z)
const -> std::vector<double>;
349 auto calculateAccumulatedLengths(
const lanelet::ConstLineString3d &)
const -> std::vector<double>;
351 auto calculateSegmentDistances(
const lanelet::ConstLineString3d &)
const -> std::vector<double>;
353 auto excludeSubtypeLanelets(
354 const std::vector<std::pair<double, lanelet::Lanelet>> &,
const char subtype[])
const
355 -> std::vector<std::pair<double, lanelet::Lanelet>>;
357 auto filterLanelets(
const lanelet::Lanelets &,
const char subtype[])
const -> lanelet::Lanelets;
359 auto findNearestIndexPair(
360 const std::vector<double> & accumulated_lengths,
const double target_length)
const
361 -> std::pair<std::size_t, std::size_t>;
363 auto generateFineCenterline(
const lanelet::ConstLanelet &,
const double resolution)
const
364 -> lanelet::LineString3d;
371 auto getNextRoadShoulderLanelet(
const lanelet::Id)
const -> lanelet::Ids;
373 auto getPreviousRoadShoulderLanelet(
const lanelet::Id)
const -> lanelet::Ids;
375 auto getStopLines()
const -> lanelet::ConstLineStrings3d;
377 auto getStopLinesOnPath(
const lanelet::Ids &)
const -> lanelet::ConstLineStrings3d;
379 auto getTrafficLightRegulatoryElementsOnPath(
const lanelet::Ids &)
const
380 -> std::vector<std::shared_ptr<const lanelet::autoware::AutowareTrafficLight>>;
382 auto getTrafficLights(
const lanelet::Id traffic_light_id)
const
383 -> std::vector<lanelet::AutowareTrafficLightConstPtr>;
385 auto getTrafficSignRegulatoryElementsOnPath(
const lanelet::Ids &)
const
386 -> std::vector<std::shared_ptr<const lanelet::TrafficSign>>;
388 auto getTrafficSignRegulatoryElements()
const
389 -> std::vector<std::shared_ptr<const lanelet::TrafficSign>>;
391 auto getVectorFromPose(
const geometry_msgs::msg::Pose &,
const double magnitude)
const
392 -> geometry_msgs::msg::Vector3;
394 auto mapCallback(
const autoware_auto_mapping_msgs::msg::HADMapBin &)
const -> void;
396 auto overwriteLaneletsCenterline() -> void;
398 auto resamplePoints(
const lanelet::ConstLineString3d &,
const std::int32_t num_segments)
const
399 -> lanelet::BasicPoints3d;
401 auto toPoint2d(
const geometry_msgs::msg::Point &)
const -> lanelet::BasicPoint2d;
403 auto toPolygon(
const lanelet::ConstLineString3d &)
const
404 -> std::vector<geometry_msgs::msg::Point>;
Definition: hdmap_utils.hpp:62
auto getStopLineIds() const -> lanelet::Ids
Definition: hdmap_utils.cpp:1800
auto getTangentVector(const lanelet::Id, const double s) const -> std::optional< geometry_msgs::msg::Vector3 >
Definition: hdmap_utils.cpp:1444
auto getPreviousLanelets(const lanelet::Id, const double distance=100) const -> lanelet::Ids
Definition: hdmap_utils.cpp:763
auto toMapPoints(const lanelet::Id, const std::vector< double > &s) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:1401
auto getNextLaneletIds(const lanelet::Ids &) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1043
auto getLateralDistance(const traffic_simulator_msgs::msg::LaneletPose &from, const traffic_simulator_msgs::msg::LaneletPose &to, bool allow_lane_change=false) const -> std::optional< double >
Definition: hdmap_utils.cpp:1458
HdMapUtils(const boost::filesystem::path &, const geographic_msgs::msg::GeoPoint &)
Definition: hdmap_utils.cpp:59
auto getFollowingLanelets(const lanelet::Id lanelet_id, const lanelet::Ids &candidate_lanelet_ids, const double distance=100, const bool include_self=true) const -> lanelet::Ids
Definition: hdmap_utils.cpp:796
auto matchToLane(const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &, const bool include_crosswalk, const double matching_distance=1.0, const double reduction_ratio=0.8) const -> std::optional< lanelet::Id >
Definition: hdmap_utils.cpp:502
auto getCenterPointsSpline(const lanelet::Id) const -> std::shared_ptr< math::geometry::CatmullRomSpline >
Definition: hdmap_utils.cpp:891
auto getLeftLaneletIds(const lanelet::Id, const traffic_simulator_msgs::msg::EntityType &, const bool include_opposite_direction=true) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1180
auto getRoute(const lanelet::Id from, const lanelet::Id to, bool allow_lane_change=false) const -> lanelet::Ids
Definition: hdmap_utils.cpp:863
auto getDistanceToTrafficLightStopLine(const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: hdmap_utils.cpp:1925
auto getStopLineIdsOnPath(const lanelet::Ids &route_lanelets) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1809
auto getTrafficLightStopLinesPoints(const lanelet::Id traffic_light_id) const -> std::vector< std::vector< geometry_msgs::msg::Point >>
Definition: hdmap_utils.cpp:1852
auto getCollisionPointInLaneCoordinate(const lanelet::Id lanelet_id, const lanelet::Id crossing_lanelet_id) const -> std::optional< double >
Definition: hdmap_utils.cpp:321
auto getDistanceToStopLine(const lanelet::Ids &route_lanelets, const math::geometry::CatmullRomSplineInterface &spline) const -> std::optional< double >
Definition: hdmap_utils.cpp:2014
auto getTrafficLightIds() const -> lanelet::Ids
Definition: hdmap_utils.cpp:1075
auto isInRoute(const lanelet::Id, const lanelet::Ids &route) const -> bool
Definition: hdmap_utils.cpp:789
auto toLaneletPose(const geometry_msgs::msg::Pose &, const bool include_crosswalk, const double matching_distance=1.0) const -> std::optional< traffic_simulator_msgs::msg::LaneletPose >
Definition: hdmap_utils.cpp:545
auto getRightOfWayLaneletIds(const lanelet::Ids &) const -> std::unordered_map< lanelet::Id, lanelet::Ids >
Definition: hdmap_utils.cpp:1706
auto filterLaneletIds(const lanelet::Ids &, const char subtype[]) const -> lanelet::Ids
Definition: hdmap_utils.cpp:242
auto getLaneChangeableLaneletId(const lanelet::Id, const traffic_simulator::lane_change::Direction) const -> std::optional< lanelet::Id >
Definition: hdmap_utils.cpp:739
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:1304
auto getStopLinePolygon(const lanelet::Id) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:1874
auto getHeight(const traffic_simulator_msgs::msg::LaneletPose &) const -> double
Definition: hdmap_utils.cpp:315
auto canonicalizeLaneletPose(const traffic_simulator_msgs::msg::LaneletPose &) const -> std::tuple< std::optional< traffic_simulator_msgs::msg::LaneletPose >, std::optional< lanelet::Id >>
Definition: hdmap_utils.cpp:159
auto getTrafficLightRegulatoryElement(const lanelet::Id) const -> lanelet::TrafficLight::Ptr
Definition: hdmap_utils.cpp:2178
auto canChangeLane(const lanelet::Id from, const lanelet::Id to) const -> bool
Definition: hdmap_utils.cpp:1450
auto getAlongLaneletPose(const traffic_simulator_msgs::msg::LaneletPose &from, const double along) const -> traffic_simulator_msgs::msg::LaneletPose
Definition: hdmap_utils.cpp:1130
auto isTrafficLightRegulatoryElement(const lanelet::Id) const -> bool
Definition: hdmap_utils.cpp:2171
auto getTrafficLightBulbPosition(const lanelet::Id traffic_light_id, const std::string &) const -> std::optional< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:1094
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:404
auto getSpeedLimit(const lanelet::Ids &) const -> double
Definition: hdmap_utils.cpp:701
auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch=true) const -> geometry_msgs::msg::PoseStamped
Definition: hdmap_utils.cpp:1412
auto getPreviousLaneletIds(const lanelet::Ids &) const -> lanelet::Ids
Definition: hdmap_utils.cpp:986
auto getLeftBound(const lanelet::Id) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:1168
auto getTrafficLightRegulatoryElementIDsFromTrafficLight(const lanelet::Id) const -> lanelet::Ids
Definition: hdmap_utils.cpp:2186
auto getTrafficLightIdsOnPath(const lanelet::Ids &route_lanelets) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1889
auto getLaneletLength(const lanelet::Id) const -> double
Definition: hdmap_utils.cpp:951
auto getRightBound(const lanelet::Id) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:1174
auto isTrafficLight(const lanelet::Id) const -> bool
Definition: hdmap_utils.cpp:2157
auto getLaneletIds() const -> lanelet::Ids
Definition: hdmap_utils.cpp:217
auto toLaneletPoses(const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance=5.0, const bool include_opposite_direction=true) const -> std::vector< traffic_simulator_msgs::msg::LaneletPose >
Definition: hdmap_utils.cpp:648
auto getConflictingLaneIds(const lanelet::Ids &) const -> lanelet::Ids
Definition: hdmap_utils.cpp:370
auto generateMarker() const -> visualization_msgs::msg::MarkerArray
Definition: hdmap_utils.cpp:1608
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:669
auto getTrafficLightStopLineIds(const lanelet::Id traffic_light_id) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1840
auto getCenterPoints(const lanelet::Ids &) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:898
auto getLaneletPolygon(const lanelet::Id) const -> std::vector< geometry_msgs::msg::Point >
Definition: hdmap_utils.cpp:226
auto getAllCanonicalizedLaneletPoses(const traffic_simulator_msgs::msg::LaneletPose &) const -> std::vector< traffic_simulator_msgs::msg::LaneletPose >
Definition: hdmap_utils.cpp:93
auto getLongitudinalDistance(const traffic_simulator_msgs::msg::LaneletPose &from, const traffic_simulator_msgs::msg::LaneletPose &to, bool allow_lane_change=false) const -> std::optional< double >
Definition: hdmap_utils.cpp:1503
auto getRightLaneletIds(lanelet::Id, traffic_simulator_msgs::msg::EntityType, bool include_opposite_direction=true) const -> lanelet::Ids
Definition: hdmap_utils.cpp:1215
auto toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin
Definition: hdmap_utils.cpp:1585
auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets
Definition: hdmap_utils.cpp:2148
auto isInLanelet(const lanelet::Id, const double s) const -> bool
Definition: hdmap_utils.cpp:1396
auto getConflictingCrosswalkIds(const lanelet::Ids &) const -> lanelet::Ids
Definition: hdmap_utils.cpp:384
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:277
auto insertMarkerArray(visualization_msgs::msg::MarkerArray &, const visualization_msgs::msg::MarkerArray &) const -> void
Definition: hdmap_utils.cpp:1601
Definition: cache.hpp:127
Definition: catmull_rom_spline_interface.hpp:30
Definition: hermite_curve.hpp:32
LaneletType
Definition: hdmap_utils.hpp:59
auto distance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2)
Definition: detection_sensor.cpp:34
TrajectoryShape
Definition: lane_change.hpp:31
Direction
Definition: lane_change.hpp:29
traffic_simulator_msgs::msg::LaneletPose LaneletPose
Definition: lanelet_pose.hpp:22
std::string string
Definition: junit5.hpp:26
parameters for behavior plugin
Definition: lane_change.hpp:75