Ros2 v0.8.0 map loader and lanelet2 extension (autowarefoundation#279)

map/lanelet2_extension/CMakeLists.txt

@@ -60,6 +60,8 @@ target_link_libraries(lanelet2_extension_sample
 ament_auto_add_executable(autoware_lanelet2_validation src/validation.cpp)
 add_dependencies(autoware_lanelet2_validation lanelet2_extension_lib)
 target_link_libraries(autoware_lanelet2_validation
+  ${catkin_LIBRARIES}
+  ${PUGIXML_LIBRARIES}
   lanelet2_extension_lib
 )
 

map/lanelet2_extension/README.md

@@ -12,7 +12,7 @@ However, there are some custom tags that is used by the parser.
 
 Currently, this includes:
 * overwriting x,y values with `local_x` and `local_y` tags.
-* reading `<MapMetaInfo>` tag wich contains information about map format version and map version.
+* reading `<MapMetaInfo>` tag which contains information about map format version and map version.
 
 The parser is registered as "autoware_osm_handler" as lanelet parser
 
@@ -41,21 +41,21 @@ Currently it contains following conversions:
 This module contains functions to retrieve various information from maps.
 e.g. crosswalks, trafficlights, stoplines
 
-#### Utilties
+#### Utilities
 This module contains other useful functions related to Lanelet.
 e.g. matching waypoint with lanelets
 
 ### Visualization
 Visualization contains functions to convert lanelet objects into visualization marker messages.
-Currenly it contains following conversions:
+Currently it contains following conversions:
 * lanelet::Lanelet to Triangle Markers
 * lanelet::LineString to LineStrip Markers
 * TrafficLights to Triangle Markers
 
 ## Nodes
 ### lanelet2_extension_sample
 Code for this explains how this lanelet2_extension library is used.
-The executable is not meanto to do anything. 
+The executable is not meant to do anything.
 
 ### autoware_lanelet2_extension
 This node checks if an .osm file follows the Autoware version of Lanelet2 format.

map/lanelet2_extension/docs/extra_regulatory_elements.md

@@ -1,7 +1,7 @@
 # Extra Regulatory Elements
 
 ## Detection Area
-This regualtory element specifies region of interest which vehicle must pay attention whenever it is driving along the associated lanelet. When there are any obstcle in the detection area, vehicle must stop at specified stopline
+This regulatory element specifies region of interest which vehicle must pay attention whenever it is driving along the associated lanelet. When there are any obstacle in the detection area, vehicle must stop at specified stopline
 
 * refers: refers to detection area polygon. There could be multiple detection areas registered to a single regulatory element.
 * refline: refers to stop line of the detection area

map/lanelet2_extension/docs/lanelet2_format_extension.md

@@ -22,7 +22,7 @@ Here is an example osm syntax for node object.
 ```
 
 ### TrafficLights
-Default Lanelet2 format uses LineString(`way`) or Polygon class to represent the shape of a traffic light. For Autoware, traffic light objects must be represented only by LineString to avoid confusion, where start point is at bottom left edge and end point is at bottom right edge. Also, "height" tag must be added in order to represent the size in verticle direction(not the position). 
+Default Lanelet2 format uses LineString(`way`) or Polygon class to represent the shape of a traffic light. For Autoware, traffic light objects must be represented only by LineString to avoid confusion, where start point is at bottom left edge and end point is at bottom right edge. Also, "height" tag must be added in order to represent the size in vertical direction (not the position).
 
 The Following image illustrates how LineString is used to represent shape of Traffic Light in Autoware.
 <img src="./traffic_light.png" width="600">
@@ -39,7 +39,7 @@ Here is an example osm syntax for traffic light object.
 </way>
 ```
 
-### Turn Diretions
+### Turn Directions
 Users must add "turn_direction" tags to lanelets within intersections to indicate vehicle's turning direction. You do not need this tags for lanelets that are not in intersections. If you do not have this tag, Autoware will not be able to light up turning indicators. 
 This tags only take following values:
 * left
@@ -50,7 +50,7 @@ Following image illustrates how lanelets should be tagged.
 
 <img src="./turn_direction.png" width="600">
 
-Here is an example of osm sytax for lanelets in intersections. 
+Here is an example of osm syntax for lanelets in intersections.
 ```
 <relation id='1' visible='true' version='1'>
   <member type='way' ref='2' role='left' />
@@ -68,7 +68,7 @@ Here is an example of osm sytax for lanelets in intersections.
 Following tags are optional tags that you may want to add depending on how you want to use your map in Autoware. 
 
 ### Meta Info
-Users may add the `MetaInfo` element to their OSM file to indicate format version and map version of their OSM file. This information is not meant to influence Autoware vehicle's behavior, but is published as ROS message so that developers could know which map was used from ROSBAG log files. MetaInfo elements exists in the same hiararchy with `node`, `way`, and `relation` elements, otherwise JOSM wouldn't be able to load the file correctly.
+Users may add the `MetaInfo` element to their OSM file to indicate format version and map version of their OSM file. This information is not meant to influence Autoware vehicle's behavior, but is published as ROS message so that developers could know which map was used from ROSBAG log files. MetaInfo elements exists in the same hierarchy with `node`, `way`, and `relation` elements, otherwise JOSM wouldn't be able to load the file correctly.
 
 Here is an example of MetaInfo in osm file:
 ```
@@ -120,7 +120,7 @@ Following image illustrates how "light_bulbs" LineString should be created.
 
 <img src="./light_bulbs.png" width="400">
 
-Here is an exmaple of osm syntax for light_bulb object:
+Here is an example of osm syntax for light_bulb object:
 ```
 <node id=1 version='1' lat='49.00541994701' lon='8.41565013855'>
   <tag k='ele' v='5'/>

map/lanelet2_extension/include/lanelet2_extension/projection/mgrs_projector.hpp

@@ -103,7 +103,7 @@ class MGRSProjector : public Projector
   std::string mgrs_code_;
 
   /**
-   * mgrs grid code that was last projected in previous foward function.
+   * mgrs grid code that was last projected in previous forward function.
    * reverse function will use this if isMGRSCodeSet() returns false.
    */
   mutable std::string projected_grid_;

map/lanelet2_extension/include/lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp

@@ -84,10 +84,10 @@ class AutowareTrafficLight : public lanelet::TrafficLight
 static lanelet::RegisterRegulatoryElement<AutowareTrafficLight> regAutowareTraffic;
 
 // moved to lanelet2_extension/lib/autoware_traffic_light.cpp to avoid multiple
-// defintion errors
+// definition errors
 /*
 #if __cplusplus < 201703L
-constexpr char AutowareTrafficLight::RuleName[];      // instanciate string in
+constexpr char AutowareTrafficLight::RuleName[];      // instantiate string in
 cpp file #endif
 */
 }  // namespace autoware

map/lanelet2_extension/include/lanelet2_extension/utility/query.hpp

@@ -100,9 +100,17 @@ std::vector<lanelet::AutowareTrafficLightConstPtr> autowareTrafficLights(
  */
 std::vector<lanelet::DetectionAreaConstPtr> detectionAreas(const lanelet::ConstLanelets & lanelets);
 
+// query all obstacle polygons in lanelet2 map
+lanelet::ConstPolygons3d getAllObstaclePolygons(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
 // query all parking lots in lanelet2 map
 lanelet::ConstPolygons3d getAllParkingLots(const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
 
+// query all pedestrian markings in lanelet2 map
+lanelet::ConstLineStrings3d getAllPedestrianMarkings(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr);
+
 // query all parking spaces in lanelet2 map
 lanelet::ConstLineStrings3d getAllParkingSpaces(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr);

map/lanelet2_extension/include/lanelet2_extension/utility/utilities.hpp

@@ -19,6 +19,7 @@
 #ifndef LANELET2_EXTENSION_UTILITY_UTILITIES_H
 #define LANELET2_EXTENSION_UTILITY_UTILITIES_H
 
+#include <rclcpp/rclcpp.hpp>
 #include "geometry_msgs/msg/point.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
@@ -33,6 +34,8 @@ namespace utils
 {
 lanelet::LineString3d generateFineCenterline(
   const lanelet::ConstLanelet & lanelet_obj, const double resolution = 5.0);
+lanelet::ConstLineString3d getCenterlineWithOffset(
+  const lanelet::ConstLanelet & lanelet_obj, const double offset, const double resolution = 5.0);
 
 /**
  * @brief  Apply a patch for centerline because the original implementation
@@ -48,6 +51,9 @@ lanelet::ConstLanelets getConflictingLanelets(
 bool lineStringWithWidthToPolygon(
   const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon);
 
+bool lineStringToPolygon(
+  const lanelet::ConstLineString3d & linestring, lanelet::ConstPolygon3d * polygon);
+
 double getLaneletLength2d(const lanelet::ConstLanelet & lanelet);
 double getLaneletLength3d(const lanelet::ConstLanelet & lanelet);
 double getLaneletLength2d(const lanelet::ConstLanelets & lanelet_sequence);
@@ -63,6 +69,9 @@ lanelet::CompoundPolygon3d getPolygonFromArcLength(
   const lanelet::ConstLanelets & lanelets, const double s1, const double s2);
 double getLaneletAngle(
   const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & search_point);
+bool isInLanelet(
+  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelet & lanelet,
+  const double radius = 0.0);
 
 }  // namespace utils
 }  // namespace lanelet

map/lanelet2_extension/include/lanelet2_extension/visualization/visualization.hpp

@@ -65,14 +65,14 @@ void lanelet2Polygon(const lanelet::ConstLanelet & ll, geometry_msgs::msg::Polyg
 /**
  * [lineString2Marker creates marker to visualize shape of linestring]
  * @param ls         [input linestring]
- * @param line_strip [output marker message]
+ * @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]
  * @param lss        [thickness of the marker]
  */
 void lineString2Marker(
-  const lanelet::ConstLineString3d ls, visualization_msgs::msg::Marker * line_strip,
+  const lanelet::ConstLineString3d ls, visualization_msgs::msg::Marker * marker,
   const std::string frame_id, const std::string ns, const std_msgs::msg::ColorRGBA c,
   const float lss = 0.1);
 /**
@@ -166,15 +166,48 @@ visualization_msgs::msg::MarkerArray trafficLightsAsTriangleMarkerArray(
  * @param  duration     [lifetime of the marker]
  */
 visualization_msgs::msg::MarkerArray detectionAreasAsMarkerArray(
-  const std::vector<lanelet::DetectionAreaConstPtr> & da_reg_elems,
-  const std_msgs::msg::ColorRGBA c,
+  const std::vector<lanelet::DetectionAreaConstPtr> & da_reg_elems, const std_msgs::msg::ColorRGBA c,
   const rclcpp::Duration duration = rclcpp::Duration(0, 0));
 
+/**
+ * [pedestrianMarkingsAsMarkerArray creates marker array to visualize pedestrian markings]
+ * @param  pedestrian_markings [pedestrian marking polygon]
+ * @param  c            [color of the marker]
+ */
+visualization_msgs::msg::MarkerArray pedestrianMarkingsAsMarkerArray(
+  const lanelet::ConstLineStrings3d & pedestrian_markings, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [parkingLotsAsMarkerArray creates marker array to visualize parking lots]
+ * @param  parking_lots [parking lot polygon]
+ * @param  c            [color of the marker]
+ */
 visualization_msgs::msg::MarkerArray parkingLotsAsMarkerArray(
   const lanelet::ConstPolygons3d & parking_lots, const std_msgs::msg::ColorRGBA & c);
+
+/**
+ * [parkingSpacesAsMarkerArray creates marker array to visualize parking spaces]
+ * @param  parking_spaces [parking space line string]
+ * @param  c            [color of the marker]
+ */
 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 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 double scale = 0.5);
+
+visualization_msgs::msg::MarkerArray obstaclePolygonsAsMarkerArray(
+  const lanelet::ConstPolygons3d & obstacle_polygons, const std_msgs::msg::ColorRGBA & c);
+
 }  // namespace visualization
 }  // namespace lanelet
 

map/lanelet2_extension/lib/autoware_osm_parser.cpp

@@ -72,7 +72,7 @@ void AutowareOsmParser::parseVersions(
   auto result = doc.load_file(filename.c_str());
   if (!result) {
     throw lanelet::ParseError(
-            std::string("Errors occured while parsing osm file: ") + result.description());
+      std::string("Errors occurred while parsing osm file: ") + result.description());
   }
 
   auto osmNode = doc.child("osm");

map/lanelet2_extension/lib/autoware_traffic_light.cpp

@@ -142,7 +142,7 @@ bool AutowareTrafficLight::removeLightBulbs(const LineStringOrPolygon3d & primit
 }
 
 #if __cplusplus < 201703L
-constexpr char AutowareTrafficLight::RuleName[];  // instanciate string in cpp file
+constexpr char AutowareTrafficLight::RuleName[];  // instantiate string in cpp file
 #endif
 
 }  // namespace autoware

map/lanelet2_extension/lib/detection_area.cpp

@@ -150,7 +150,7 @@ void DetectionArea::setStopLine(const LineString3d & stopLine)
 void DetectionArea::removeStopLine() {parameters()[RoleName::RefLine] = {};}
 
 #if __cplusplus < 201703L
-constexpr char DetectionArea::RuleName[];  // instanciate string in cpp file
+constexpr char DetectionArea::RuleName[];  // instantiate string in cpp file
 #endif
 
 }  // namespace autoware

map/lanelet2_extension/lib/query.cpp

@@ -50,7 +50,7 @@ namespace lanelet
 namespace utils
 {
 // returns all lanelets in laneletLayer - don't know how to convert
-// PrimitveLayer<Lanelets> -> std::vector<Lanelets>
+// PrimitiveLayer<Lanelets> -> std::vector<Lanelets>
 lanelet::ConstLanelets query::laneletLayer(const lanelet::LaneletMapConstPtr & ll_map)
 {
   lanelet::ConstLanelets lanelets;
@@ -186,6 +186,19 @@ std::vector<lanelet::DetectionAreaConstPtr> query::detectionAreas(
   return da_reg_elems;
 }
 
+lanelet::ConstPolygons3d query::getAllObstaclePolygons(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstPolygons3d obstacle_polygons;
+  for (const auto & poly : lanelet_map_ptr->polygonLayer) {
+    const std::string type = poly.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type.compare("obstacle") == 0) {
+      obstacle_polygons.push_back(poly);
+    }
+  }
+  return obstacle_polygons;
+}
+
 lanelet::ConstPolygons3d query::getAllParkingLots(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
 {
@@ -199,6 +212,19 @@ lanelet::ConstPolygons3d query::getAllParkingLots(
   return parking_lots;
 }
 
+lanelet::ConstLineStrings3d query::getAllPedestrianMarkings(
+  const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
+{
+  lanelet::ConstLineStrings3d pedestrian_markings;
+  for (const auto & ls : lanelet_map_ptr->lineStringLayer) {
+    const std::string type = ls.attributeOr(lanelet::AttributeName::Type, "none");
+    if (type.compare("pedestrian_marking") == 0) {
+      pedestrian_markings.push_back(ls);
+    }
+  }
+  return pedestrian_markings;
+}
+
 lanelet::ConstLineStrings3d query::getAllParkingSpaces(
   const lanelet::LaneletMapConstPtr & lanelet_map_ptr)
 {
@@ -427,17 +453,17 @@ std::vector<lanelet::ConstLineString3d> query::stopLinesLanelets(
   return stoplines;
 }
 
-// return all stop and ref lines from a given lanel
+// return all stop and ref lines from a given lanelet
 std::vector<lanelet::ConstLineString3d> query::stopLinesLanelet(const lanelet::ConstLanelet ll)
 {
   std::vector<lanelet::ConstLineString3d> stoplines;
 
-  // find stop lines referened by right ofway reg. elems.
+  // find stop lines referenced by right of way reg. elems.
   std::vector<std::shared_ptr<const lanelet::RightOfWay>> right_of_way_reg_elems =
     ll.regulatoryElementsAs<const lanelet::RightOfWay>();
 
   if (right_of_way_reg_elems.size() > 0) {
-    // lanelet has a right of way elem elemetn
+    // lanelet has a right of way elem element
     for (auto j = right_of_way_reg_elems.begin(); j < right_of_way_reg_elems.end(); j++) {
       if ((*j)->getManeuver(ll) == lanelet::ManeuverType::Yield) {
         // lanelet has a yield reg. elem.
@@ -452,7 +478,7 @@ std::vector<lanelet::ConstLineString3d> query::stopLinesLanelet(const lanelet::C
     ll.regulatoryElementsAs<const lanelet::TrafficLight>();
 
   if (traffic_light_reg_elems.size() > 0) {
-    // lanelet has a traffic light elem elemetn
+    // lanelet has a traffic light elem element
     for (auto j = traffic_light_reg_elems.begin(); j < traffic_light_reg_elems.end(); j++) {
       lanelet::Optional<lanelet::ConstLineString3d> traffic_light_stopline_opt = (*j)->stopLine();
       if (!!traffic_light_stopline_opt) {stoplines.push_back(traffic_light_stopline_opt.get());}
@@ -684,7 +710,7 @@ std::vector<std::deque<lanelet::ConstLanelet>> getSucceedingLaneletSequencesRecu
   }
 
   for (const auto & next_lanelet : next_lanelets) {
-    // get lanelet sequnce after next_lanelet
+    // get lanelet sequence after next_lanelet
     auto tmp_lanelet_sequences =
       getSucceedingLaneletSequencesRecursive(graph, next_lanelet, length - lanelet_length);
     for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) {
@@ -699,27 +725,27 @@ std::vector<std::deque<lanelet::ConstLanelet>> getPreceedingLaneletSequencesRecu
   const routing::RoutingGraphPtr & graph, const lanelet::ConstLanelet & lanelet,
   const double length)
 {
-  std::vector<std::deque<lanelet::ConstLanelet>> preceeding_lanelet_sequences;
+  std::vector<std::deque<lanelet::ConstLanelet>> preceding_lanelet_sequences;
 
   const auto prev_lanelets = graph->previous(lanelet);
   const double lanelet_length = utils::getLaneletLength3d(lanelet);
 
   // end condition of the recursive function
   if (prev_lanelets.empty() || lanelet_length >= length) {
-    preceeding_lanelet_sequences.push_back({lanelet});
-    return preceeding_lanelet_sequences;
+    preceding_lanelet_sequences.push_back({lanelet});
+    return preceding_lanelet_sequences;
   }
 
   for (const auto & prev_lanelet : prev_lanelets) {
-    // get lanelet sequnce after prev_lanelet
+    // get lanelet sequence after prev_lanelet
     auto tmp_lanelet_sequences =
       getPreceedingLaneletSequencesRecursive(graph, prev_lanelet, length - lanelet_length);
     for (auto & tmp_lanelet_sequence : tmp_lanelet_sequences) {
       tmp_lanelet_sequence.push_back(lanelet);
-      preceeding_lanelet_sequences.push_back(tmp_lanelet_sequence);
+      preceding_lanelet_sequences.push_back(tmp_lanelet_sequence);
     }
   }
-  return preceeding_lanelet_sequences;
+  return preceding_lanelet_sequences;
 }
 
 std::vector<lanelet::ConstLanelets> query::getSucceedingLaneletSequences(

map/lanelet2_extension/lib/road_marking.cpp

@@ -71,7 +71,7 @@ void RoadMarking::setRoadMarking(const LineString3d & road_marking)
 void RoadMarking::removeRoadMarking() {parameters()[RoleName::Refers] = {};}
 
 #if __cplusplus < 201703L
-constexpr char RoadMarking::RuleName[];  // instanciate string in cpp file
+constexpr char RoadMarking::RuleName[];  // instantiate string in cpp file
 #endif
 
 }  // namespace autoware