From 4661d4f46c8cdc10e10d40429062c3dea1885ea6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 20 May 2024 16:26:23 +0900 Subject: [PATCH] feat!(lanelet2_extension): introduce API versioning along with format_version Signed-off-by: Mamoru Sobue --- .../io/autoware_osm_parser.hpp | 2 + .../projection/mgrs_projector.hpp | 4 + .../transverse_mercator_projector.hpp | 4 + .../regulatory_elements/Forward.hpp | 8 + .../autoware_traffic_light.hpp | 4 + .../regulatory_elements/crosswalk.hpp | 4 + .../regulatory_elements/detection_area.hpp | 4 + .../regulatory_elements/no_parking_area.hpp | 4 + .../regulatory_elements/no_stopping_area.hpp | 4 + .../regulatory_elements/road_marking.hpp | 4 + .../regulatory_elements/speed_bump.hpp | 4 + .../virtual_traffic_light.hpp | 4 + .../lanelet2_extension/utility/query.hpp | 72 +- .../include/lanelet2_extension/version.hpp | 32 + .../visualization/visualization.hpp | 300 +++---- tmp/lanelet2_extension/lib/query.cpp | 218 +++--- tmp/lanelet2_extension/lib/visualization.cpp | 736 +++++++++--------- 17 files changed, 751 insertions(+), 657 deletions(-) create mode 100644 tmp/lanelet2_extension/include/lanelet2_extension/version.hpp diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp index 8645ddbf..50d04c65 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/io/autoware_osm_parser.hpp @@ -19,6 +19,8 @@ // NOLINTBEGIN(readability-identifier-naming) +#include + #include #include diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp index 37b6bc07..ced91994 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp @@ -30,6 +30,9 @@ namespace lanelet::projection { + +inline namespace v1 +{ class MGRSProjector : public Projector { public: @@ -108,6 +111,7 @@ class MGRSProjector : public Projector */ mutable std::string projected_grid_; }; +} // namespace v1 } // namespace lanelet::projection diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/projection/transverse_mercator_projector.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/projection/transverse_mercator_projector.hpp index f8693b90..d214c089 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/projection/transverse_mercator_projector.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/projection/transverse_mercator_projector.hpp @@ -27,6 +27,9 @@ namespace lanelet::projection { + +inline namespace v1 +{ class TransverseMercatorProjector : public Projector { public: @@ -53,6 +56,7 @@ class TransverseMercatorProjector : public Projector double origin_y_; double central_meridian_; }; +} // namespace v1 } // namespace lanelet::projection diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/Forward.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/Forward.hpp index 16b814d3..fc0b47d8 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/Forward.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/Forward.hpp @@ -25,6 +25,8 @@ namespace lanelet::autoware { +inline namespace v1 +{ class AutowareTrafficLight; class Crosswalk; class DetectionArea; @@ -33,11 +35,15 @@ class NoStoppingArea; class RoadMarking; class SpeedBump; class VirtualTrafficLight; +} // namespace v1 } // namespace lanelet::autoware namespace lanelet { + +inline namespace v1 +{ using TrafficSignConstPtr = std::shared_ptr; using TrafficLightConstPtr = std::shared_ptr; using AutowareTrafficLightConstPtr = std::shared_ptr; @@ -47,6 +53,8 @@ using NoStoppingAreaConstPtr = std::shared_ptr; using SpeedBumpConstPtr = std::shared_ptr; using CrosswalkConstPtr = std::shared_ptr; +} // namespace v1 + } // namespace lanelet // NOLINTEND(readability-identifier-naming) diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp index f175d23a..0bf3d485 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp @@ -28,6 +28,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class AutowareTrafficLight : public lanelet::TrafficLight { public: @@ -83,6 +86,7 @@ class AutowareTrafficLight : public lanelet::TrafficLight friend class RegisterRegulatoryElement; explicit AutowareTrafficLight(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp index f6cec39f..81cba03e 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/crosswalk.hpp @@ -25,6 +25,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class Crosswalk : public lanelet::RegulatoryElement { public: @@ -84,6 +87,7 @@ class Crosswalk : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit Crosswalk(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp index 698ab384..da08013e 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/detection_area.hpp @@ -27,6 +27,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class DetectionArea : public lanelet::RegulatoryElement { public: @@ -88,6 +91,7 @@ class DetectionArea : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit DetectionArea(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp index 8ef43fa8..b1d33dc1 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_parking_area.hpp @@ -25,6 +25,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class NoParkingArea : public lanelet::RegulatoryElement { public: @@ -64,6 +67,7 @@ class NoParkingArea : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit NoParkingArea(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp index eaf263ba..9c1d355b 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/no_stopping_area.hpp @@ -25,6 +25,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class NoStoppingArea : public lanelet::RegulatoryElement { public: @@ -86,6 +89,7 @@ class NoStoppingArea : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit NoStoppingArea(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp index 0fc4142c..4bbf88ff 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/road_marking.hpp @@ -25,6 +25,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class RoadMarking : public lanelet::RegulatoryElement { public: @@ -64,6 +67,7 @@ class RoadMarking : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit RoadMarking(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/speed_bump.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/speed_bump.hpp index 59c8d551..a9cc685e 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/speed_bump.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/speed_bump.hpp @@ -27,6 +27,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class SpeedBump : public lanelet::RegulatoryElement { public: @@ -66,6 +69,7 @@ class SpeedBump : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit SpeedBump(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp index fdbb6334..629a15a5 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/regulatory_elements/virtual_traffic_light.hpp @@ -25,6 +25,9 @@ namespace lanelet::autoware { + +inline namespace v1 +{ class VirtualTrafficLight : public lanelet::RegulatoryElement { public: @@ -71,6 +74,7 @@ class VirtualTrafficLight : public lanelet::RegulatoryElement friend class RegisterRegulatoryElement; explicit VirtualTrafficLight(const lanelet::RegulatoryElementDataPtr & data); }; +} // namespace v1 } // namespace lanelet::autoware diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp index b094a2b5..9d89409e 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/utility/query.hpp @@ -39,37 +39,9 @@ namespace lanelet::utils::query { -/** - * [laneletLayer converts laneletLayer into lanelet vector] - * @param ll_Map [input lanelet map] - * @return [all lanelets in the map] - */ -lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapConstPtr & ll_Map); - -/** - * [subtypeLanelets extracts Lanelet that has given subtype attribute] - * @param lls [input lanelets with various subtypes] - * @param subtype [subtype of lanelets to be retrieved (e.g. - * lanelet::AttributeValueString::Road)] - * @return [lanelets with given subtype] - */ -lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets & lls, const char subtype[]); - -/** - * [crosswalkLanelets extracts crosswalk lanelets] - * @param lls [input lanelets with various subtypes] - * @return [crosswalk lanelets] - */ -lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets & lls); -lanelet::ConstLanelets walkwayLanelets(const lanelet::ConstLanelets & lls); - -/** - * [roadLanelets extracts road lanelets] - * @param lls [input lanelets with subtype road] - * @return [road lanelets] - */ -lanelet::ConstLanelets roadLanelets(const lanelet::ConstLanelets & lls); +inline namespace v1 +{ /** * [shoulderLanelets extracts shoulder lanelets] * @param lls [input lanelets with subtype shoulder] @@ -129,13 +101,17 @@ std::vector speedBumps(const lanelet::ConstLanelets */ std::vector crosswalks(const lanelet::ConstLanelets & lanelets); +/** + * [crosswalkLanelets extracts crosswalk lanelets] + * @param lls [input lanelets with various subtypes] + * @return [crosswalk lanelets] + */ +lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets & lls); +lanelet::ConstLanelets walkwayLanelets(const lanelet::ConstLanelets & lls); + // query all curbstones in lanelet2 map lanelet::ConstLineStrings3d curbstones(const lanelet::LaneletMapConstPtr & lanelet_map_ptr); -// query all polygons that has given type in lanelet2 map -lanelet::ConstPolygons3d getAllPolygonsByType( - const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type); - // query all obstacle polygons in lanelet2 map lanelet::ConstPolygons3d getAllObstaclePolygons( const lanelet::LaneletMapConstPtr & lanelet_map_ptr); @@ -227,6 +203,34 @@ std::vector stopLinesLanelet(const lanelet::ConstLan */ std::vector stopSignStopLines( const lanelet::ConstLanelets & lanelets, const std::string & stop_sign_id = "stop_sign"); +} // namespace v1 + +/** + * [laneletLayer converts laneletLayer into lanelet vector] + * @param ll_Map [input lanelet map] + * @return [all lanelets in the map] + */ +lanelet::ConstLanelets laneletLayer(const lanelet::LaneletMapConstPtr & ll_Map); + +/** + * [subtypeLanelets extracts Lanelet that has given subtype attribute] + * @param lls [input lanelets with various subtypes] + * @param subtype [subtype of lanelets to be retrieved (e.g. + * lanelet::AttributeValueString::Road)] + * @return [lanelets with given subtype] + */ +lanelet::ConstLanelets subtypeLanelets(const lanelet::ConstLanelets & lls, const char subtype[]); + +/** + * [roadLanelets extracts road lanelets] + * @param lls [input lanelets with subtype road] + * @return [road lanelets] + */ +lanelet::ConstLanelets roadLanelets(const lanelet::ConstLanelets & lls); + +// query all polygons that has given type in lanelet2 map +lanelet::ConstPolygons3d getAllPolygonsByType( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type); ConstLanelets getLaneletsWithinRange( const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point, diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/version.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/version.hpp new file mode 100644 index 00000000..4a88ea82 --- /dev/null +++ b/tmp/lanelet2_extension/include/lanelet2_extension/version.hpp @@ -0,0 +1,32 @@ +// Copyright 2015-2024 Autoware Foundation. 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. +// +// Authors: Mamoru Sobue + +#ifndef LANELET2_EXTENSION__VERSION_HPP_ +#define LANELET2_EXTENSION__VERSION_HPP_ + +// NOLINTBEGIN(readability-identifier-naming) + +namespace lanelet::autoware +{ +enum class Version : int { + none = 0, + v1, +}; + +static constexpr Version version = Version::v1; +} // namespace lanelet::autoware + +#endif // LANELET2_EXTENSION__VERSION_HPP_ diff --git a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp index c904c43a..29fd2c97 100644 --- a/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp +++ b/tmp/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp @@ -37,142 +37,9 @@ namespace lanelet::visualization { -/** - * [lanelet2Triangle converts lanelet into vector of triangles. Used for - * triangulation] - * @param ll [input lanelet] - * @param triangles [array of polygon message, each containing 3 vertices] - */ -void lanelet2Triangle( - const lanelet::ConstLanelet & ll, std::vector * triangles); - -/** - * [polygon2Triangle converts polygon into vector of triangles] - * @param polygon [input polygon] - * @param triangles [array of polygon message, each containing 3 vertices] - */ -void polygon2Triangle( - const geometry_msgs::msg::Polygon & polygon, - std::vector * triangles); - -/** - * [lanelet2Polygon converts lanelet into a polygon] - * @param ll [input lanelet] - * @param polygon [polygon message containing shape of the input lanelet.] - */ -void lanelet2Polygon(const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon); - -/** - * [initLineStringMarker initializes marker to visualize shape of linestring] - * @param marker [output marker message] - * @param frame_id [frame id of the marker] - * @param ns [namespace of the marker] - * @param c [color of the marker] - */ -void initLineStringMarker( - visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, - const std_msgs::msg::ColorRGBA & c); - -/** - * [pushLineStringMarker pushes marker vertices to visualize shape of linestring] - * @param marker [output marker message] - * @param ls [input linestring] - * @param c [color of the marker] - * @param lss [thickness of the marker] - */ -void pushLineStringMarker( - visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, - const std_msgs::msg::ColorRGBA & c, const float lss = 0.1); - -/** - * [initArrowsMarker initializes marker to visualize arrows with TRIANGLE_LIST] - * @param marker [output marker message] - * @param frame_id [frame id of the marker] - * @param ns [namespace of the marker] - * @param c [color of the marker] - */ -void initArrowsMarker( - visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, - const std_msgs::msg::ColorRGBA & c); - -/** - * [pushArrowsMarker pushes marker to visualize arrows with TRIANGLE_LIST] - * @param marker [output marker message] - * @param ls [input linestring] - * @param c [color of the marker] - */ -void pushArrowsMarker( - visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, - const std_msgs::msg::ColorRGBA & c); - -/** - * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic - * lights] - * @param marker [created marker] - * @param ns [namespace of the marker] - * @param duration [lifetime of the marker] - */ -void initTrafficLightTriangleMarker( - visualization_msgs::msg::Marker * marker, const std::string & ns, - const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); - -/** - * [pushTrafficLightTriangleMarker pushes marker vertices to visualize shape of traffic - * lights] - * @param marker [created marker] - * @param ls [linestring that represents traffic light shape] - * @param cl [color of the marker] - * @param scale [scale of the marker] - */ -void pushTrafficLightTriangleMarker( - visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, - const std_msgs::msg::ColorRGBA & cl, const double scale = 1.0); - -/** - * [laneletsBoundaryAsMarkerArray create marker array to visualize shape of - * boundaries of lanelets] - * @param lanelets [input lanelets] - * @param c [color of the boundary] - * @param viz_centerline [flag to visualize centerline or not] - * @return [created marker array] - */ -visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray( - const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c, - const bool viz_centerline, const std::string & additional_namespace = ""); -/** - * [laneletsAsTriangleMarkerArray create marker array to visualize shape of the - * lanelet] - * @param ns [namespace of the marker] - * @param lanelets [input lanelets] - * @param c [color of the marker] - * @return [created marker] - */ -visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray( - const std::string & ns, const lanelet::ConstLanelets & lanelets, - const std_msgs::msg::ColorRGBA & c); - -/** - * [laneletDirectionAsMarkerArray create marker array to visualize direction of - * the lanelet] - * @param lanelets [input lanelets] - * @return [created marker array] - */ -visualization_msgs::msg::MarkerArray laneletDirectionAsMarkerArray( - const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace = ""); - -/** - * [lineStringsAsMarkerArray creates marker array to visualize shape of - * linestrings] - * @param line_strings [input linestrings] - * @param name_space [namespace of the marker] - * @param c [color of the marker] - * @param lss [thickness of the marker] - * @return [created marker array] - */ -visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray( - const std::vector & line_strings, const std::string & name_space, - const std_msgs::msg::ColorRGBA & c, const float lss); +inline namespace v1 +{ /** * [autowareTrafficLightsAsMarkerArray creates marker array to visualize traffic * lights] @@ -308,19 +175,6 @@ visualization_msgs::msg::MarkerArray parkingLotsAsMarkerArray( visualization_msgs::msg::MarkerArray parkingSpacesAsMarkerArray( const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c); -/** - * [detectionAreasAsMarkerArray creates marker array to visualize lanelet_id] - * @param road_lanelets [road lanelets] - * @param c [color of the marker] - * @param duration [lifetime of the marker] - * @param ns [namespace of the marker] - * @param scale [scale of the marker] - * @return visualization_msgs::msg::MarkerArray - */ -visualization_msgs::msg::MarkerArray generateLaneletIdMarker( - const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c, - const std::string & ns = "lanelet_id", const double scale = 0.5); - visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray( const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c); @@ -363,6 +217,156 @@ visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaForRunOutAsMarker visualization_msgs::msg::MarkerArray hatchedRoadMarkingsAreaAsMarkerArray( const lanelet::ConstPolygons3d & hatched_road_markings_area, const std_msgs::msg::ColorRGBA & area_color, const std_msgs::msg::ColorRGBA & line_color); +} // namespace v1 + +/** + * [lanelet2Triangle converts lanelet into vector of triangles. Used for + * triangulation] + * @param ll [input lanelet] + * @param triangles [array of polygon message, each containing 3 vertices] + */ +void lanelet2Triangle( + const lanelet::ConstLanelet & ll, std::vector * triangles); + +/** + * [polygon2Triangle converts polygon into vector of triangles] + * @param polygon [input polygon] + * @param triangles [array of polygon message, each containing 3 vertices] + */ +void polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon, + std::vector * triangles); + +/** + * [lanelet2Polygon converts lanelet into a polygon] + * @param ll [input lanelet] + * @param polygon [polygon message containing shape of the input lanelet.] + */ +void lanelet2Polygon(const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon); + +/** + * [initLineStringMarker initializes marker to visualize shape of linestring] + * @param marker [output marker message] + * @param frame_id [frame id of the marker] + * @param ns [namespace of the marker] + * @param c [color of the marker] + */ +void initLineStringMarker( + visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c); + +/** + * [pushLineStringMarker pushes marker vertices to visualize shape of linestring] + * @param marker [output marker message] + * @param ls [input linestring] + * @param c [color of the marker] + * @param lss [thickness of the marker] + */ +void pushLineStringMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & c, const float lss = 0.1); + +/** + * [initArrowsMarker initializes marker to visualize arrows with TRIANGLE_LIST] + * @param marker [output marker message] + * @param frame_id [frame id of the marker] + * @param ns [namespace of the marker] + * @param c [color of the marker] + */ +void initArrowsMarker( + visualization_msgs::msg::Marker * marker, const std::string & frame_id, const std::string & ns, + const std_msgs::msg::ColorRGBA & c); + +/** + * [pushArrowsMarker pushes marker to visualize arrows with TRIANGLE_LIST] + * @param marker [output marker message] + * @param ls [input linestring] + * @param c [color of the marker] + */ +void pushArrowsMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & c); + +/** + * [initTrafficLightTriangleMarker initializes marker to visualize shape of traffic + * lights] + * @param marker [created marker] + * @param ns [namespace of the marker] + * @param duration [lifetime of the marker] + */ +void initTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const std::string & ns, + const rclcpp::Duration & duration = rclcpp::Duration(0, 0)); + +/** + * [pushTrafficLightTriangleMarker pushes marker vertices to visualize shape of traffic + * lights] + * @param marker [created marker] + * @param ls [linestring that represents traffic light shape] + * @param cl [color of the marker] + * @param scale [scale of the marker] + */ +void pushTrafficLightTriangleMarker( + visualization_msgs::msg::Marker * marker, const lanelet::ConstLineString3d & ls, + const std_msgs::msg::ColorRGBA & cl, const double scale = 1.0); + +/** + * [laneletsBoundaryAsMarkerArray create marker array to visualize shape of + * boundaries of lanelets] + * @param lanelets [input lanelets] + * @param c [color of the boundary] + * @param viz_centerline [flag to visualize centerline or not] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray laneletsBoundaryAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c, + const bool viz_centerline, const std::string & additional_namespace = ""); +/** + * [laneletsAsTriangleMarkerArray create marker array to visualize shape of the + * lanelet] + * @param ns [namespace of the marker] + * @param lanelets [input lanelets] + * @param c [color of the marker] + * @return [created marker] + */ +visualization_msgs::msg::MarkerArray laneletsAsTriangleMarkerArray( + const std::string & ns, const lanelet::ConstLanelets & lanelets, + const std_msgs::msg::ColorRGBA & c); + +/** + * [laneletDirectionAsMarkerArray create marker array to visualize direction of + * the lanelet] + * @param lanelets [input lanelets] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray laneletDirectionAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace = ""); + +/** + * [lineStringsAsMarkerArray creates marker array to visualize shape of + * linestrings] + * @param line_strings [input linestrings] + * @param name_space [namespace of the marker] + * @param c [color of the marker] + * @param lss [thickness of the marker] + * @return [created marker array] + */ +visualization_msgs::msg::MarkerArray lineStringsAsMarkerArray( + const std::vector & line_strings, const std::string & name_space, + const std_msgs::msg::ColorRGBA & c, const float lss); + +/** + * [detectionAreasAsMarkerArray creates marker array to visualize lanelet_id] + * @param road_lanelets [road lanelets] + * @param c [color of the marker] + * @param duration [lifetime of the marker] + * @param ns [namespace of the marker] + * @param scale [scale of the marker] + * @return visualization_msgs::msg::MarkerArray + */ +visualization_msgs::msg::MarkerArray generateLaneletIdMarker( + const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c, + const std::string & ns = "lanelet_id", const double scale = 0.5); } // namespace lanelet::visualization diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp index a505c12e..03b5b9e3 100644 --- a/tmp/lanelet2_extension/lib/query.cpp +++ b/tmp/lanelet2_extension/lib/query.cpp @@ -52,62 +52,17 @@ using lanelet::utils::to2D; namespace lanelet::utils { -// returns all lanelets in laneletLayer - don't know how to convert -// PrimitiveLayer -> std::vector -lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map) -{ - lanelet::ConstLanelets lanelets; - if (!ll_map) { - std::cerr << "No map received!"; - return lanelets; - } - - for (const auto & li : ll_map->laneletLayer) { - lanelets.push_back(li); - } - - return lanelets; -} -lanelet::ConstLanelets query::subtypeLanelets( - const lanelet::ConstLanelets & lls, const char subtype[]) +namespace query { - lanelet::ConstLanelets subtype_lanelets; - - for (const auto & ll : lls) { - if (ll.hasAttribute(lanelet::AttributeName::Subtype)) { - const lanelet::Attribute & attr = ll.attribute(lanelet::AttributeName::Subtype); - if (attr.value() == subtype) { - subtype_lanelets.push_back(ll); - } - } - } - - return subtype_lanelets; -} - -lanelet::ConstLanelets query::crosswalkLanelets(const lanelet::ConstLanelets & lls) -{ - return query::subtypeLanelets(lls, lanelet::AttributeValueString::Crosswalk); -} - -lanelet::ConstLanelets query::walkwayLanelets(const lanelet::ConstLanelets & lls) +inline namespace v1 { - return query::subtypeLanelets(lls, lanelet::AttributeValueString::Walkway); -} - -lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets & lls) +lanelet::ConstLanelets shoulderLanelets(const lanelet::ConstLanelets & lls) { - return query::subtypeLanelets(lls, lanelet::AttributeValueString::Road); + return subtypeLanelets(lls, "road_shoulder"); } -lanelet::ConstLanelets query::shoulderLanelets(const lanelet::ConstLanelets & lls) -{ - return query::subtypeLanelets(lls, "road_shoulder"); -} - -std::vector query::trafficLights( - const lanelet::ConstLanelets & lanelets) +std::vector trafficLights(const lanelet::ConstLanelets & lanelets) { std::vector tl_reg_elems; @@ -133,7 +88,7 @@ std::vector query::trafficLights( return tl_reg_elems; } -std::vector query::autowareTrafficLights( +std::vector autowareTrafficLights( const lanelet::ConstLanelets & lanelets) { std::vector tl_reg_elems; @@ -162,8 +117,7 @@ std::vector query::autowareTrafficLights( return tl_reg_elems; } -std::vector query::detectionAreas( - const lanelet::ConstLanelets & lanelets) +std::vector detectionAreas(const lanelet::ConstLanelets & lanelets) { std::vector da_reg_elems; @@ -191,7 +145,7 @@ std::vector query::detectionAreas( return da_reg_elems; } -std::vector query::noStoppingAreas( +std::vector noStoppingAreas( const lanelet::ConstLanelets & lanelets) { std::vector no_reg_elems; @@ -220,8 +174,7 @@ std::vector query::noStoppingAreas( return no_reg_elems; } -std::vector query::noParkingAreas( - const lanelet::ConstLanelets & lanelets) +std::vector noParkingAreas(const lanelet::ConstLanelets & lanelets) { std::vector no_pa_reg_elems; @@ -249,7 +202,7 @@ std::vector query::noParkingAreas( return no_pa_reg_elems; } -std::vector query::speedBumps(const lanelet::ConstLanelets & lanelets) +std::vector speedBumps(const lanelet::ConstLanelets & lanelets) { std::vector sb_reg_elems; @@ -277,7 +230,7 @@ std::vector query::speedBumps(const lanelet::ConstLa return sb_reg_elems; } -std::vector query::crosswalks(const lanelet::ConstLanelets & lanelets) +std::vector crosswalks(const lanelet::ConstLanelets & lanelets) { std::vector cw_reg_elems; @@ -305,7 +258,17 @@ std::vector query::crosswalks(const lanelet::ConstLa return cw_reg_elems; } -lanelet::ConstLineStrings3d query::curbstones(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +lanelet::ConstLanelets crosswalkLanelets(const lanelet::ConstLanelets & lls) +{ + return subtypeLanelets(lls, lanelet::AttributeValueString::Crosswalk); +} + +lanelet::ConstLanelets walkwayLanelets(const lanelet::ConstLanelets & lls) +{ + return subtypeLanelets(lls, lanelet::AttributeValueString::Walkway); +} + +lanelet::ConstLineStrings3d curbstones(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstLineStrings3d curbstones; for (const auto & ls : lanelet_map_ptr->lineStringLayer) { @@ -317,21 +280,7 @@ lanelet::ConstLineStrings3d query::curbstones(const lanelet::LaneletMapConstPtr return curbstones; } -lanelet::ConstPolygons3d query::getAllPolygonsByType( - const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type) -{ - lanelet::ConstPolygons3d polygons; - for (const auto & poly : lanelet_map_ptr->polygonLayer) { - const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none"); - if (type == polygon_type) { - polygons.push_back(poly); - } - } - return polygons; -} - -lanelet::ConstPolygons3d query::getAllObstaclePolygons( - const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +lanelet::ConstPolygons3d getAllObstaclePolygons(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstPolygons3d obstacle_polygons; for (const auto & poly : lanelet_map_ptr->polygonLayer) { @@ -343,8 +292,7 @@ lanelet::ConstPolygons3d query::getAllObstaclePolygons( return obstacle_polygons; } -lanelet::ConstPolygons3d query::getAllParkingLots( - const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +lanelet::ConstPolygons3d getAllParkingLots(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstPolygons3d parking_lots; for (const auto & poly : lanelet_map_ptr->polygonLayer) { @@ -356,8 +304,7 @@ lanelet::ConstPolygons3d query::getAllParkingLots( return parking_lots; } -lanelet::ConstLineStrings3d query::getAllPartitions( - const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +lanelet::ConstLineStrings3d getAllPartitions(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstLineStrings3d partitions; for (const auto & ls : lanelet_map_ptr->lineStringLayer) { @@ -369,7 +316,7 @@ lanelet::ConstLineStrings3d query::getAllPartitions( return partitions; } -lanelet::ConstLineStrings3d query::getAllFences(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +lanelet::ConstLineStrings3d getAllFences(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstLineStrings3d fences; for (const auto & ls : lanelet_map_ptr->lineStringLayer) { @@ -381,7 +328,7 @@ lanelet::ConstLineStrings3d query::getAllFences(const lanelet::LaneletMapConstPt return fences; } -lanelet::ConstLineStrings3d query::getAllPedestrianPolygonMarkings( +lanelet::ConstLineStrings3d getAllPedestrianPolygonMarkings( const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstLineStrings3d pedestrian_polygon_markings; @@ -394,7 +341,7 @@ lanelet::ConstLineStrings3d query::getAllPedestrianPolygonMarkings( return pedestrian_polygon_markings; } -lanelet::ConstLineStrings3d query::getAllPedestrianLineMarkings( +lanelet::ConstLineStrings3d getAllPedestrianLineMarkings( const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstLineStrings3d pedestrian_line_markings; @@ -407,8 +354,7 @@ lanelet::ConstLineStrings3d query::getAllPedestrianLineMarkings( return pedestrian_line_markings; } -lanelet::ConstLineStrings3d query::getAllParkingSpaces( - const lanelet::LaneletMapConstPtr & lanelet_map_ptr) +lanelet::ConstLineStrings3d getAllParkingSpaces(const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { lanelet::ConstLineStrings3d parking_spaces; for (const auto & ls : lanelet_map_ptr->lineStringLayer) { @@ -420,18 +366,17 @@ lanelet::ConstLineStrings3d query::getAllParkingSpaces( return parking_spaces; } -bool query::getLinkedLanelet( +bool getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::LaneletMapConstPtr & lanelet_map_ptr, lanelet::ConstLanelet * linked_lanelet) { - const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); - const auto & all_road_lanelets = query::roadLanelets(all_lanelets); - const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); - return query::getLinkedLanelet( - parking_space, all_road_lanelets, all_parking_lots, linked_lanelet); + const auto & all_lanelets = laneletLayer(lanelet_map_ptr); + const auto & all_road_lanelets = roadLanelets(all_lanelets); + const auto & all_parking_lots = getAllParkingLots(lanelet_map_ptr); + return getLinkedLanelet(parking_space, all_road_lanelets, all_parking_lots, linked_lanelet); } -bool query::getLinkedLanelet( +bool getLinkedLanelet( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstLanelet * linked_lanelet) @@ -454,18 +399,18 @@ bool query::getLinkedLanelet( return true; } -lanelet::ConstLanelets query::getLinkedLanelets( +lanelet::ConstLanelets getLinkedLanelets( const lanelet::ConstLineString3d & parking_space, const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { - const auto & all_lanelets = query::laneletLayer(lanelet_map_ptr); - const auto & all_road_lanelets = query::roadLanelets(all_lanelets); - const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + const auto & all_lanelets = laneletLayer(lanelet_map_ptr); + const auto & all_road_lanelets = roadLanelets(all_lanelets); + const auto & all_parking_lots = getAllParkingLots(lanelet_map_ptr); - return query::getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); + return getLinkedLanelets(parking_space, all_road_lanelets, all_parking_lots); } -lanelet::ConstLanelets query::getLinkedLanelets( +lanelet::ConstLanelets getLinkedLanelets( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstLanelets & all_road_lanelets, const lanelet::ConstPolygons3d & all_parking_lots) @@ -509,7 +454,7 @@ lanelet::ConstLanelets query::getLinkedLanelets( } // get overlapping lanelets -lanelet::ConstLanelets query::getLinkedLanelets( +lanelet::ConstLanelets getLinkedLanelets( const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLanelets & all_road_lanelets) { lanelet::ConstLanelets linked_lanelets; @@ -523,15 +468,15 @@ lanelet::ConstLanelets query::getLinkedLanelets( return linked_lanelets; } -lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( +lanelet::ConstLineStrings3d getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::LaneletMapConstPtr & lanelet_map_ptr) { - const auto & all_parking_spaces = query::getAllParkingSpaces(lanelet_map_ptr); - const auto & all_parking_lots = query::getAllParkingLots(lanelet_map_ptr); + const auto & all_parking_spaces = getAllParkingSpaces(lanelet_map_ptr); + const auto & all_parking_lots = getAllParkingLots(lanelet_map_ptr); return getLinkedParkingSpaces(lanelet, all_parking_spaces, all_parking_lots); } -lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( +lanelet::ConstLineStrings3d getLinkedParkingSpaces( const lanelet::ConstLanelet & lanelet, const lanelet::ConstLineStrings3d & all_parking_spaces, const lanelet::ConstPolygons3d & all_parking_lots) { @@ -574,7 +519,7 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( } // get overlapping parking lot -bool query::getLinkedParkingLot( +bool getLinkedParkingLot( const lanelet::ConstLanelet & lanelet, const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) { @@ -590,7 +535,7 @@ bool query::getLinkedParkingLot( } // get overlapping parking lot -bool query::getLinkedParkingLot( +bool getLinkedParkingLot( const lanelet::BasicPoint2d & current_position, const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) { @@ -606,7 +551,7 @@ bool query::getLinkedParkingLot( } // get overlapping parking lot -bool query::getLinkedParkingLot( +bool getLinkedParkingLot( const lanelet::ConstLineString3d & parking_space, const lanelet::ConstPolygons3d & all_parking_lots, lanelet::ConstPolygon3d * linked_parking_lot) { @@ -621,7 +566,7 @@ bool query::getLinkedParkingLot( return false; } -lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( +lanelet::ConstLineStrings3d getLinkedParkingSpaces( const lanelet::ConstPolygon3d & parking_lot, const lanelet::ConstLineStrings3d & all_parking_spaces) { @@ -637,14 +582,13 @@ lanelet::ConstLineStrings3d query::getLinkedParkingSpaces( } // return all stop lines and ref lines from a given set of lanelets -std::vector query::stopLinesLanelets( - const lanelet::ConstLanelets & lanelets) +std::vector stopLinesLanelets(const lanelet::ConstLanelets & lanelets) { std::vector stoplines; for (const auto & ll : lanelets) { std::vector ll_stoplines; - ll_stoplines = query::stopLinesLanelet(ll); + ll_stoplines = stopLinesLanelet(ll); stoplines.insert(stoplines.end(), ll_stoplines.begin(), ll_stoplines.end()); } @@ -652,7 +596,7 @@ std::vector query::stopLinesLanelets( } // return all stop and ref lines from a given lanelet -std::vector query::stopLinesLanelet(const lanelet::ConstLanelet & ll) +std::vector stopLinesLanelet(const lanelet::ConstLanelet & ll) { std::vector stoplines; @@ -703,7 +647,7 @@ std::vector query::stopLinesLanelet(const lanelet::C return stoplines; } -std::vector query::stopSignStopLines( +std::vector stopSignStopLines( const lanelet::ConstLanelets & lanelets, const std::string & stop_sign_id) { std::vector stoplines; @@ -739,6 +683,60 @@ std::vector query::stopSignStopLines( } return stoplines; } +} // namespace v1 +} // namespace query + +// returns all lanelets in laneletLayer - don't know how to convert +// PrimitiveLayer -> std::vector +lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map) +{ + lanelet::ConstLanelets lanelets; + if (!ll_map) { + std::cerr << "No map received!"; + return lanelets; + } + + for (const auto & li : ll_map->laneletLayer) { + lanelets.push_back(li); + } + + return lanelets; +} + +lanelet::ConstLanelets query::subtypeLanelets( + const lanelet::ConstLanelets & lls, const char subtype[]) +{ + lanelet::ConstLanelets subtype_lanelets; + + for (const auto & ll : lls) { + if (ll.hasAttribute(lanelet::AttributeName::Subtype)) { + const lanelet::Attribute & attr = ll.attribute(lanelet::AttributeName::Subtype); + if (attr.value() == subtype) { + subtype_lanelets.push_back(ll); + } + } + } + + return subtype_lanelets; +} + +lanelet::ConstLanelets query::roadLanelets(const lanelet::ConstLanelets & lls) +{ + return query::subtypeLanelets(lls, lanelet::AttributeValueString::Road); +} + +lanelet::ConstPolygons3d query::getAllPolygonsByType( + const lanelet::LaneletMapConstPtr & lanelet_map_ptr, const std::string & polygon_type) +{ + lanelet::ConstPolygons3d polygons; + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none"); + if (type == polygon_type) { + polygons.push_back(poly); + } + } + return polygons; +} ConstLanelets query::getLaneletsWithinRange( const lanelet::ConstLanelets & lanelets, const lanelet::BasicPoint2d & search_point, diff --git a/tmp/lanelet2_extension/lib/visualization.cpp b/tmp/lanelet2_extension/lib/visualization.cpp index 7ae33727..dc1a4b37 100644 --- a/tmp/lanelet2_extension/lib/visualization.cpp +++ b/tmp/lanelet2_extension/lib/visualization.cpp @@ -348,156 +348,12 @@ void pushPolygonMarker( namespace lanelet { -void visualization::lanelet2Triangle( - const lanelet::ConstLanelet & ll, std::vector * triangles) -{ - if (triangles == nullptr) { - std::cerr << __FUNCTION__ << ": triangles is null pointer!" << std::endl; - return; - } - - triangles->clear(); - geometry_msgs::msg::Polygon ll_poly; - lanelet2Polygon(ll, &ll_poly); - polygon2Triangle(ll_poly, triangles); -} - -// NOLINTBEGIN(readability-function-cognitive-complexity) -void visualization::polygon2Triangle( - const geometry_msgs::msg::Polygon & polygon, std::vector * triangles) -{ - geometry_msgs::msg::Polygon poly = polygon; - if (!isClockWise(poly)) { - std::reverse(poly.points.begin(), poly.points.end()); - } - - // ear clipping: find smallest internal angle in polygon - int N = static_cast(poly.points.size()); - - // array of angles for each vertex - std::vector is_acute_angle; - is_acute_angle.assign(N, false); - for (int i = 0; i < N; i++) { - geometry_msgs::msg::Point32 p0; - geometry_msgs::msg::Point32 p1; - geometry_msgs::msg::Point32 p2; - - adjacentPoints(i, N, poly, &p0, &p1, &p2); - is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2); - } - - // start ear clipping - while (N >= 3) { - int clipped_vertex = -1; - - for (int i = 0; i < N; i++) { - const bool theta = is_acute_angle.at(i); - if (theta) { - geometry_msgs::msg::Point32 p0; - geometry_msgs::msg::Point32 p1; - geometry_msgs::msg::Point32 p2; - adjacentPoints(i, N, poly, &p0, &p1, &p2); - - int j_begin = (i + 2) % N; - int j_end = (i - 1 + N) % N; - bool is_ear = true; - for (int j = j_begin; j != j_end; j = (j + 1) % N) { - if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) { - is_ear = false; - break; - } - } - - if (is_ear) { - clipped_vertex = i; - break; - } - } - } - if (clipped_vertex < 0 || clipped_vertex >= N) { - // print in yellow to indicate warning - std::cerr << "\033[1;33mCould not find valid vertex for ear clipping triangulation. " - "Triangulation result might be invalid\033[0m" - << std::endl; - clipped_vertex = 0; - } - - // create triangle - geometry_msgs::msg::Point32 p0; - geometry_msgs::msg::Point32 p1; - geometry_msgs::msg::Point32 p2; - adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); - geometry_msgs::msg::Polygon triangle; - triangle.points.push_back(p0); - triangle.points.push_back(p1); - triangle.points.push_back(p2); - triangles->push_back(triangle); - - // remove vertex of center of angle - auto it = poly.points.begin(); - std::advance(it, clipped_vertex); - poly.points.erase(it); - - // remove from angle list - auto it_angle = is_acute_angle.begin(); - std::advance(it_angle, clipped_vertex); - is_acute_angle.erase(it_angle); - - // update angle list - N = static_cast(poly.points.size()); - if (clipped_vertex == N) { - clipped_vertex = 0; - } - adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); - is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2); - - int i_prev = (clipped_vertex == 0) ? N - 1 : clipped_vertex - 1; - adjacentPoints(i_prev, N, poly, &p0, &p1, &p2); - is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2); - } -} -// NOLINTEND(readability-function-cognitive-complexity) -void visualization::lanelet2Polygon( - const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon) +namespace visualization { - if (polygon == nullptr) { - std::cerr << __FUNCTION__ << ": polygon is null pointer!" << std::endl; - return; - } - - lanelet::CompoundPolygon3d ll_poly = ll.polygon3d(); - - polygon->points.clear(); - polygon->points.reserve(ll_poly.size()); - - for (const auto & pt : ll_poly) { - geometry_msgs::msg::Point32 pt32; - utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32); - polygon->points.push_back(pt32); - } -} - -visualization_msgs::msg::MarkerArray visualization::laneletDirectionAsMarkerArray( - const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace) +inline namespace v1 { - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; - initLaneletDirectionMarker(&marker, additional_namespace + "lanelet direction"); - - for (const auto & ll : lanelets) { - if (ll.hasAttribute(std::string("turn_direction"))) { - pushLaneletDirectionMarker(&marker, ll); - } - } - if (marker.points.empty()) { - return marker_array; - } - marker_array.markers.push_back(marker); - return marker_array; -} - -visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarkerArray( +visualization_msgs::msg::MarkerArray autowareTrafficLightsAsMarkerArray( const std::vector & tl_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) { @@ -508,14 +364,14 @@ visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarke visualization_msgs::msg::Marker marker_tri; visualization_msgs::msg::Marker marker_sph; initLightMarker(&marker_sph, "traffic_light"); - visualization::initTrafficLightTriangleMarker(&marker_tri, "traffic_light_triangle", duration); + initTrafficLightTriangleMarker(&marker_tri, "traffic_light_triangle", duration); for (const auto & tl : tl_reg_elems) { const auto lights = tl->trafficLights(); for (const auto & lsp : lights) { if (lsp.isLineString()) { // traffic lights can either polygons or linestrings lanelet::ConstLineString3d ls = static_cast(lsp); - visualization::pushTrafficLightTriangleMarker(&marker_tri, ls, c, scale); + pushTrafficLightTriangleMarker(&marker_tri, ls, c, scale); } } marker_tri.id++; @@ -538,7 +394,7 @@ visualization_msgs::msg::MarkerArray visualization::autowareTrafficLightsAsMarke return tl_marker_array; } -visualization_msgs::msg::MarkerArray visualization::generateTrafficLightRegulatoryElementIdMaker( +visualization_msgs::msg::MarkerArray generateTrafficLightRegulatoryElementIdMaker( const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) { @@ -578,7 +434,7 @@ visualization_msgs::msg::MarkerArray visualization::generateTrafficLightRegulato return tl_id_marker_array; } -visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( +visualization_msgs::msg::MarkerArray generateTrafficLightIdMaker( const std::vector & tl_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) { @@ -630,7 +486,35 @@ visualization_msgs::msg::MarkerArray visualization::generateTrafficLightIdMaker( return tl_id_marker_array; } -visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray( +visualization_msgs::msg::MarkerArray trafficLightsAsTriangleMarkerArray( + const std::vector & tl_reg_elems, + const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) +{ + // convert to to an array of linestrings and publish as marker array using + // existing function + + std::vector line_strings; + visualization_msgs::msg::Marker marker; + initTrafficLightTriangleMarker(&marker, "traffic_light_triangle", duration); + + for (const auto & tl : tl_reg_elems) { + lanelet::LineString3d ls; + + const auto lights = tl->trafficLights(); + for (const auto & lsp : lights) { + if (lsp.isLineString()) { // traffic lights can either polygons or linestrings + lanelet::ConstLineString3d ls = static_cast(lsp); + pushTrafficLightTriangleMarker(&marker, ls, c, scale); + } + } + } + + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.push_back(marker); + return marker_array; +} + +visualization_msgs::msg::MarkerArray detectionAreasAsMarkerArray( const std::vector & da_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) { @@ -669,7 +553,7 @@ visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray( line_c.g = 0.5; line_c.b = 0.5; line_c.a = 0.999; - visualization::initLineStringMarker(&line_marker, "map", "detection_area_stopline", line_c); + initLineStringMarker(&line_marker, "map", "detection_area_stopline", line_c); for (const auto & da_reg_elem : da_reg_elems) { marker.points.clear(); @@ -698,14 +582,14 @@ visualization_msgs::msg::MarkerArray visualization::detectionAreasAsMarkerArray( marker_array.markers.push_back(marker); // stop line visualization - visualization::pushLineStringMarker(&line_marker, da_reg_elem->stopLine(), line_c, 0.5); + pushLineStringMarker(&line_marker, da_reg_elem->stopLine(), line_c, 0.5); } // for regulatory elements marker_array.markers.push_back(line_marker); return marker_array; } -visualization_msgs::msg::MarkerArray visualization::noParkingAreasAsMarkerArray( +visualization_msgs::msg::MarkerArray noParkingAreasAsMarkerArray( const std::vector & no_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) { @@ -767,7 +651,7 @@ visualization_msgs::msg::MarkerArray visualization::noParkingAreasAsMarkerArray( return marker_array; } -visualization_msgs::msg::MarkerArray visualization::noStoppingAreasAsMarkerArray( +visualization_msgs::msg::MarkerArray noStoppingAreasAsMarkerArray( const std::vector & no_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) { @@ -806,7 +690,7 @@ visualization_msgs::msg::MarkerArray visualization::noStoppingAreasAsMarkerArray line_c.g = 0.5; line_c.b = 0.5; line_c.a = 0.999; - visualization::initLineStringMarker(&line_marker, "map", "no_stopping_area_stopline", line_c); + initLineStringMarker(&line_marker, "map", "no_stopping_area_stopline", line_c); for (const auto & no_reg_elem : no_reg_elems) { marker.points.clear(); @@ -836,7 +720,7 @@ visualization_msgs::msg::MarkerArray visualization::noStoppingAreasAsMarkerArray const auto & stop_line = no_reg_elem->stopLine(); // stop line visualization if (stop_line) { - visualization::pushLineStringMarker(&line_marker, stop_line.value(), line_c, 0.5); + pushLineStringMarker(&line_marker, stop_line.value(), line_c, 0.5); } } // for regulatory elements if (!line_marker.points.empty()) { @@ -845,7 +729,7 @@ visualization_msgs::msg::MarkerArray visualization::noStoppingAreasAsMarkerArray return marker_array; } -visualization_msgs::msg::MarkerArray visualization::speedBumpsAsMarkerArray( +visualization_msgs::msg::MarkerArray speedBumpsAsMarkerArray( const std::vector & sb_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) { @@ -909,7 +793,7 @@ visualization_msgs::msg::MarkerArray visualization::speedBumpsAsMarkerArray( return marker_array; } -visualization_msgs::msg::MarkerArray visualization::crosswalkAreasAsMarkerArray( +visualization_msgs::msg::MarkerArray crosswalkAreasAsMarkerArray( const std::vector & cw_reg_elems, const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration) { @@ -974,7 +858,7 @@ visualization_msgs::msg::MarkerArray visualization::crosswalkAreasAsMarkerArray( return marker_array; } -visualization_msgs::msg::MarkerArray visualization::pedestrianPolygonMarkingsAsMarkerArray( +visualization_msgs::msg::MarkerArray pedestrianPolygonMarkingsAsMarkerArray( const lanelet::ConstLineStrings3d & pedestrian_polygon_markings, const std_msgs::msg::ColorRGBA & c) { @@ -1003,7 +887,7 @@ visualization_msgs::msg::MarkerArray visualization::pedestrianPolygonMarkingsAsM return marker_array; } -visualization_msgs::msg::MarkerArray visualization::pedestrianLineMarkingsAsMarkerArray( +visualization_msgs::msg::MarkerArray pedestrianLineMarkingsAsMarkerArray( const lanelet::ConstLineStrings3d & pedestrian_line_markings, const std_msgs::msg::ColorRGBA & c) { visualization_msgs::msg::MarkerArray marker_array; @@ -1013,7 +897,7 @@ visualization_msgs::msg::MarkerArray visualization::pedestrianLineMarkingsAsMark const float lss = 0.1; // line string size visualization_msgs::msg::Marker line_marker; - visualization::initLineStringMarker(&line_marker, "map", "pedestrian_line_marking", c); + initLineStringMarker(&line_marker, "map", "pedestrian_line_marking", c); for (const auto & linestring : pedestrian_line_markings) { if ((linestring.size() < 3) && (linestring.front().id() != linestring.back().id())) { @@ -1028,7 +912,7 @@ visualization_msgs::msg::MarkerArray visualization::pedestrianLineMarkingsAsMark return marker_array; } -visualization_msgs::msg::MarkerArray visualization::parkingLotsAsMarkerArray( +visualization_msgs::msg::MarkerArray parkingLotsAsMarkerArray( const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c) { visualization_msgs::msg::MarkerArray marker_array; @@ -1046,7 +930,7 @@ visualization_msgs::msg::MarkerArray visualization::parkingLotsAsMarkerArray( } return marker_array; } -visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray( +visualization_msgs::msg::MarkerArray parkingSpacesAsMarkerArray( const lanelet::ConstLineStrings3d & parking_spaces, const std_msgs::msg::ColorRGBA & c) { visualization_msgs::msg::MarkerArray marker_array; @@ -1070,39 +954,7 @@ visualization_msgs::msg::MarkerArray visualization::parkingSpacesAsMarkerArray( return marker_array; } -visualization_msgs::msg::MarkerArray visualization::generateLaneletIdMarker( - const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c, - const std::string & ns, const double scale) -{ - visualization_msgs::msg::MarkerArray markers; - for (const auto & ll : road_lanelets) { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = "map"; - marker.header.stamp = rclcpp::Clock().now(); - marker.ns = ns; - marker.id = static_cast(ll.id()); - marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - marker.action = visualization_msgs::msg::Marker::ADD; - const auto centerline = ll.centerline(); - const size_t target_position_index = centerline.size() / 2; - const auto & target_position = centerline[target_position_index]; - marker.pose.position.x = target_position.x(); - marker.pose.position.y = target_position.y(); - marker.pose.position.z = target_position.z(); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.color = c; - marker.scale.z = scale; - marker.frame_locked = false; - marker.text = std::to_string(ll.id()); - markers.markers.push_back(marker); - } - return markers; -} - -visualization_msgs::msg::MarkerArray visualization::obstaclePolygonsAsMarkerArray( +visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray( const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c) { visualization_msgs::msg::MarkerArray marker_array; @@ -1121,49 +973,335 @@ visualization_msgs::msg::MarkerArray visualization::obstaclePolygonsAsMarkerArra return marker_array; } -visualization_msgs::msg::MarkerArray visualization::lineStringsAsMarkerArray( - const std::vector & line_strings, const std::string & name_space, - const std_msgs::msg::ColorRGBA & c, const float lss) +visualization_msgs::msg::MarkerArray intersectionAreaAsMarkerArray( + const lanelet::ConstPolygons3d & intersection_areas, const std_msgs::msg::ColorRGBA & c) { - visualization_msgs::msg::MarkerArray ls_marker_array; - if (line_strings.empty()) { - return ls_marker_array; + visualization_msgs::msg::MarkerArray marker_array; + if (intersection_areas.empty()) { + return marker_array; } - std::unordered_set added; - visualization_msgs::msg::Marker ls_marker; - visualization::initLineStringMarker(&ls_marker, "map", name_space, c); - for (const auto & ls : line_strings) { - if (!exists(added, ls.id())) { - visualization::pushLineStringMarker(&ls_marker, ls, c, lss); - added.insert(ls.id()); - } + visualization_msgs::msg::Marker marker = createPolygonMarker("intersection_area", c); + for (const auto & polygon : intersection_areas) { + pushPolygonMarker(&marker, polygon, c); } - ls_marker_array.markers.push_back(ls_marker); - return ls_marker_array; + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; } -visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArray( - const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c, - const bool viz_centerline, const std::string & additional_namespace) +visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaAsMarkerArray( + const lanelet::ConstPolygons3d & no_obstacle_segmentation_area, + const std_msgs::msg::ColorRGBA & c) { - const float lss = 0.1; // line string size - const float lss_center = static_cast(std::max(lss * 0.1, 0.02)); + visualization_msgs::msg::MarkerArray marker_array; + if (no_obstacle_segmentation_area.empty()) { + return marker_array; + } - std::unordered_set added; - visualization_msgs::msg::Marker left_line_strip; - visualization_msgs::msg::Marker right_line_strip; - visualization_msgs::msg::Marker start_bound_line_strip; - visualization_msgs::msg::Marker center_line_strip; - visualization_msgs::msg::Marker center_arrows; - visualization::initLineStringMarker( - &left_line_strip, "map", additional_namespace + "left_lane_bound", c); - visualization::initLineStringMarker( - &right_line_strip, "map", additional_namespace + "right_lane_bound", c); - visualization::initLineStringMarker( - &start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c); - visualization::initLineStringMarker( - ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); + visualization_msgs::msg::Marker marker = createPolygonMarker("no_obstacle_segmentation_area", c); + for (const auto & polygon : no_obstacle_segmentation_area) { + pushPolygonMarker(&marker, polygon, c); + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray noObstacleSegmentationAreaForRunOutAsMarkerArray( + const lanelet::ConstPolygons3d & no_obstacle_segmentation_area_for_run_out, + const std_msgs::msg::ColorRGBA & c) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (no_obstacle_segmentation_area_for_run_out.empty()) { + return marker_array; + } + + visualization_msgs::msg::Marker marker = + createPolygonMarker("no_obstacle_segmentation_area_for_run_out", c); + for (const auto & polygon : no_obstacle_segmentation_area_for_run_out) { + pushPolygonMarker(&marker, polygon, c); + } + + if (!marker.points.empty()) { + marker_array.markers.push_back(marker); + } + return marker_array; +} + +visualization_msgs::msg::MarkerArray hatchedRoadMarkingsAreaAsMarkerArray( + const lanelet::ConstPolygons3d & hatched_road_markings_area, + const std_msgs::msg::ColorRGBA & area_color, const std_msgs::msg::ColorRGBA & line_color) +{ + visualization_msgs::msg::MarkerArray marker_array; + if (hatched_road_markings_area.empty()) { + return marker_array; + } + + // polygon + visualization_msgs::msg::Marker area_marker = + createPolygonMarker("hatched_road_markings_area", area_color); + for (const auto & polygon : hatched_road_markings_area) { + pushPolygonMarker(&area_marker, polygon, area_color); + } + + if (!area_marker.points.empty()) { + marker_array.markers.push_back(area_marker); + } + + // line strings + const float lss = 0.1; // line string size + visualization_msgs::msg::Marker line_strip; + initLineStringMarker(&line_strip, "map", "hatched_road_markings_bound", line_color); + + for (const auto & polygon : hatched_road_markings_area) { + lanelet::LineString3d bound_ls(lanelet::utils::getId()); + for (const auto & point : polygon) { + bound_ls.push_back( + lanelet::Point3d(lanelet::utils::getId(), point.x(), point.y(), point.z())); + } + if (!bound_ls.empty()) { + bound_ls.push_back(bound_ls.front()); + } + pushLineStringMarker(&line_strip, bound_ls, line_color, lss); + } + if (!line_strip.points.empty()) { + marker_array.markers.push_back(line_strip); + } + + return marker_array; +} +} // namespace v1 +} // namespace visualization + +void visualization::lanelet2Triangle( + const lanelet::ConstLanelet & ll, std::vector * triangles) +{ + if (triangles == nullptr) { + std::cerr << __FUNCTION__ << ": triangles is null pointer!" << std::endl; + return; + } + + triangles->clear(); + geometry_msgs::msg::Polygon ll_poly; + lanelet2Polygon(ll, &ll_poly); + polygon2Triangle(ll_poly, triangles); +} + +// NOLINTBEGIN(readability-function-cognitive-complexity) +void visualization::polygon2Triangle( + const geometry_msgs::msg::Polygon & polygon, std::vector * triangles) +{ + geometry_msgs::msg::Polygon poly = polygon; + if (!isClockWise(poly)) { + std::reverse(poly.points.begin(), poly.points.end()); + } + + // ear clipping: find smallest internal angle in polygon + int N = static_cast(poly.points.size()); + + // array of angles for each vertex + std::vector is_acute_angle; + is_acute_angle.assign(N, false); + for (int i = 0; i < N; i++) { + geometry_msgs::msg::Point32 p0; + geometry_msgs::msg::Point32 p1; + geometry_msgs::msg::Point32 p2; + + adjacentPoints(i, N, poly, &p0, &p1, &p2); + is_acute_angle.at(i) = isAcuteAngle(p0, p1, p2); + } + + // start ear clipping + while (N >= 3) { + int clipped_vertex = -1; + + for (int i = 0; i < N; i++) { + const bool theta = is_acute_angle.at(i); + if (theta) { + geometry_msgs::msg::Point32 p0; + geometry_msgs::msg::Point32 p1; + geometry_msgs::msg::Point32 p2; + adjacentPoints(i, N, poly, &p0, &p1, &p2); + + int j_begin = (i + 2) % N; + int j_end = (i - 1 + N) % N; + bool is_ear = true; + for (int j = j_begin; j != j_end; j = (j + 1) % N) { + if (isWithinTriangle(p0, p1, p2, poly.points.at(j))) { + is_ear = false; + break; + } + } + + if (is_ear) { + clipped_vertex = i; + break; + } + } + } + if (clipped_vertex < 0 || clipped_vertex >= N) { + // print in yellow to indicate warning + std::cerr << "\033[1;33mCould not find valid vertex for ear clipping triangulation. " + "Triangulation result might be invalid\033[0m" + << std::endl; + clipped_vertex = 0; + } + + // create triangle + geometry_msgs::msg::Point32 p0; + geometry_msgs::msg::Point32 p1; + geometry_msgs::msg::Point32 p2; + adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); + geometry_msgs::msg::Polygon triangle; + triangle.points.push_back(p0); + triangle.points.push_back(p1); + triangle.points.push_back(p2); + triangles->push_back(triangle); + + // remove vertex of center of angle + auto it = poly.points.begin(); + std::advance(it, clipped_vertex); + poly.points.erase(it); + + // remove from angle list + auto it_angle = is_acute_angle.begin(); + std::advance(it_angle, clipped_vertex); + is_acute_angle.erase(it_angle); + + // update angle list + N = static_cast(poly.points.size()); + if (clipped_vertex == N) { + clipped_vertex = 0; + } + adjacentPoints(clipped_vertex, N, poly, &p0, &p1, &p2); + is_acute_angle.at(clipped_vertex) = isAcuteAngle(p0, p1, p2); + + int i_prev = (clipped_vertex == 0) ? N - 1 : clipped_vertex - 1; + adjacentPoints(i_prev, N, poly, &p0, &p1, &p2); + is_acute_angle.at(i_prev) = isAcuteAngle(p0, p1, p2); + } +} +// NOLINTEND(readability-function-cognitive-complexity) + +void visualization::lanelet2Polygon( + const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polygon * polygon) +{ + if (polygon == nullptr) { + std::cerr << __FUNCTION__ << ": polygon is null pointer!" << std::endl; + return; + } + + lanelet::CompoundPolygon3d ll_poly = ll.polygon3d(); + + polygon->points.clear(); + polygon->points.reserve(ll_poly.size()); + + for (const auto & pt : ll_poly) { + geometry_msgs::msg::Point32 pt32; + utils::conversion::toGeomMsgPt32(pt.basicPoint(), &pt32); + polygon->points.push_back(pt32); + } +} + +visualization_msgs::msg::MarkerArray visualization::laneletDirectionAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std::string & additional_namespace) +{ + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker marker; + initLaneletDirectionMarker(&marker, additional_namespace + "lanelet direction"); + + for (const auto & ll : lanelets) { + if (ll.hasAttribute(std::string("turn_direction"))) { + pushLaneletDirectionMarker(&marker, ll); + } + } + if (marker.points.empty()) { + return marker_array; + } + marker_array.markers.push_back(marker); + return marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::generateLaneletIdMarker( + const lanelet::ConstLanelets & road_lanelets, const std_msgs::msg::ColorRGBA & c, + const std::string & ns, const double scale) +{ + visualization_msgs::msg::MarkerArray markers; + for (const auto & ll : road_lanelets) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Clock().now(); + marker.ns = ns; + marker.id = static_cast(ll.id()); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + const auto centerline = ll.centerline(); + const size_t target_position_index = centerline.size() / 2; + const auto & target_position = centerline[target_position_index]; + marker.pose.position.x = target_position.x(); + marker.pose.position.y = target_position.y(); + marker.pose.position.z = target_position.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.color = c; + marker.scale.z = scale; + marker.frame_locked = false; + marker.text = std::to_string(ll.id()); + markers.markers.push_back(marker); + } + return markers; +} + +visualization_msgs::msg::MarkerArray visualization::lineStringsAsMarkerArray( + const std::vector & line_strings, const std::string & name_space, + const std_msgs::msg::ColorRGBA & c, const float lss) +{ + visualization_msgs::msg::MarkerArray ls_marker_array; + if (line_strings.empty()) { + return ls_marker_array; + } + std::unordered_set added; + visualization_msgs::msg::Marker ls_marker; + visualization::initLineStringMarker(&ls_marker, "map", name_space, c); + + for (const auto & ls : line_strings) { + if (!exists(added, ls.id())) { + visualization::pushLineStringMarker(&ls_marker, ls, c, lss); + added.insert(ls.id()); + } + } + ls_marker_array.markers.push_back(ls_marker); + return ls_marker_array; +} + +visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArray( + const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c, + const bool viz_centerline, const std::string & additional_namespace) +{ + const float lss = 0.1; // line string size + const float lss_center = static_cast(std::max(lss * 0.1, 0.02)); + + std::unordered_set added; + visualization_msgs::msg::Marker left_line_strip; + visualization_msgs::msg::Marker right_line_strip; + visualization_msgs::msg::Marker start_bound_line_strip; + visualization_msgs::msg::Marker center_line_strip; + visualization_msgs::msg::Marker center_arrows; + visualization::initLineStringMarker( + &left_line_strip, "map", additional_namespace + "left_lane_bound", c); + visualization::initLineStringMarker( + &right_line_strip, "map", additional_namespace + "right_lane_bound", c); + visualization::initLineStringMarker( + &start_bound_line_strip, "map", additional_namespace + "lane_start_bound", c); + visualization::initLineStringMarker( + ¢er_line_strip, "map", additional_namespace + "center_lane_line", c); visualization::initArrowsMarker( ¢er_arrows, "map", additional_namespace + "center_line_arrows", c); @@ -1215,34 +1353,6 @@ visualization_msgs::msg::MarkerArray visualization::laneletsBoundaryAsMarkerArra return marker_array; } -visualization_msgs::msg::MarkerArray visualization::trafficLightsAsTriangleMarkerArray( - const std::vector & tl_reg_elems, - const std_msgs::msg::ColorRGBA & c, const rclcpp::Duration & duration, const double scale) -{ - // convert to to an array of linestrings and publish as marker array using - // existing function - - std::vector line_strings; - visualization_msgs::msg::Marker marker; - visualization::initTrafficLightTriangleMarker(&marker, "traffic_light_triangle", duration); - - for (const auto & tl : tl_reg_elems) { - lanelet::LineString3d ls; - - const auto lights = tl->trafficLights(); - for (const auto & lsp : lights) { - if (lsp.isLineString()) { // traffic lights can either polygons or linestrings - lanelet::ConstLineString3d ls = static_cast(lsp); - visualization::pushTrafficLightTriangleMarker(&marker, ls, c, scale); - } - } - } - - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.push_back(marker); - return marker_array; -} - visualization_msgs::msg::MarkerArray visualization::laneletsAsTriangleMarkerArray( const std::string & ns, const lanelet::ConstLanelets & lanelets, const std_msgs::msg::ColorRGBA & c) @@ -1538,110 +1648,6 @@ void visualization::pushArrowsMarker( } } -visualization_msgs::msg::MarkerArray visualization::intersectionAreaAsMarkerArray( - const lanelet::ConstPolygons3d & intersection_areas, const std_msgs::msg::ColorRGBA & c) -{ - visualization_msgs::msg::MarkerArray marker_array; - if (intersection_areas.empty()) { - return marker_array; - } - - visualization_msgs::msg::Marker marker = createPolygonMarker("intersection_area", c); - for (const auto & polygon : intersection_areas) { - pushPolygonMarker(&marker, polygon, c); - } - - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray visualization::noObstacleSegmentationAreaAsMarkerArray( - const lanelet::ConstPolygons3d & no_obstacle_segmentation_area, - const std_msgs::msg::ColorRGBA & c) -{ - visualization_msgs::msg::MarkerArray marker_array; - if (no_obstacle_segmentation_area.empty()) { - return marker_array; - } - - visualization_msgs::msg::Marker marker = createPolygonMarker("no_obstacle_segmentation_area", c); - for (const auto & polygon : no_obstacle_segmentation_area) { - pushPolygonMarker(&marker, polygon, c); - } - - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray -visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( - const lanelet::ConstPolygons3d & no_obstacle_segmentation_area_for_run_out, - const std_msgs::msg::ColorRGBA & c) -{ - visualization_msgs::msg::MarkerArray marker_array; - if (no_obstacle_segmentation_area_for_run_out.empty()) { - return marker_array; - } - - visualization_msgs::msg::Marker marker = - createPolygonMarker("no_obstacle_segmentation_area_for_run_out", c); - for (const auto & polygon : no_obstacle_segmentation_area_for_run_out) { - pushPolygonMarker(&marker, polygon, c); - } - - if (!marker.points.empty()) { - marker_array.markers.push_back(marker); - } - return marker_array; -} - -visualization_msgs::msg::MarkerArray visualization::hatchedRoadMarkingsAreaAsMarkerArray( - const lanelet::ConstPolygons3d & hatched_road_markings_area, - const std_msgs::msg::ColorRGBA & area_color, const std_msgs::msg::ColorRGBA & line_color) -{ - visualization_msgs::msg::MarkerArray marker_array; - if (hatched_road_markings_area.empty()) { - return marker_array; - } - - // polygon - visualization_msgs::msg::Marker area_marker = - createPolygonMarker("hatched_road_markings_area", area_color); - for (const auto & polygon : hatched_road_markings_area) { - pushPolygonMarker(&area_marker, polygon, area_color); - } - - if (!area_marker.points.empty()) { - marker_array.markers.push_back(area_marker); - } - - // line strings - const float lss = 0.1; // line string size - visualization_msgs::msg::Marker line_strip; - visualization::initLineStringMarker( - &line_strip, "map", "hatched_road_markings_bound", line_color); - - for (const auto & polygon : hatched_road_markings_area) { - lanelet::LineString3d bound_ls(lanelet::utils::getId()); - for (const auto & point : polygon) { - bound_ls.push_back( - lanelet::Point3d(lanelet::utils::getId(), point.x(), point.y(), point.z())); - } - if (!bound_ls.empty()) { - bound_ls.push_back(bound_ls.front()); - } - visualization::pushLineStringMarker(&line_strip, bound_ls, line_color, lss); - } - if (!line_strip.points.empty()) { - marker_array.markers.push_back(line_strip); - } - - return marker_array; -} } // namespace lanelet // NOLINTEND(readability-identifier-naming)