Merge branch 'beta/v0.11.2' into fix/ndt_scan_matcher/delete_diagnost…

…ics_thread
tier4 · Jan 16, 2024 · a606563 · a606563
2 parents e7bc713 + f74b33d
commit a606563
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 1 deletion.
diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp
@@ -127,13 +127,14 @@ void Lanelet2MapVisualizationNode::onMapBin(
     lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings");
   std::vector<lanelet::NoParkingAreaConstPtr> no_parking_reg_elems =
     lanelet::utils::query::noParkingAreas(all_lanelets);
+  lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map);
 
   std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
     cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
     cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id,
     cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
     cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area,
-    cl_hatched_road_markings_line, cl_no_parking_areas;
+    cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -156,6 +157,7 @@ void Lanelet2MapVisualizationNode::onMapBin(
   setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5);
   setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999);
   setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5);
+  setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -242,6 +244,10 @@ void Lanelet2MapVisualizationNode::onMapBin(
     &map_marker_array,
     lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas));
 
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2));
+
   pub_marker_->publish(map_marker_array);
 }
 

diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp
@@ -1586,6 +1586,8 @@ std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
     } else {
       if (!polygon) {
         will_close_polygon = true;
+      } else if (polygon.value().id() != current_polygon.value().id()) {
+        will_close_polygon = true;
       } else {
         current_polygon_border_indices.push_back(
           get_corresponding_polygon_index(*current_polygon, bound_point.id()));
@@ -1620,6 +1622,17 @@ std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings(
             (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]);
         }
       }
+
+      if (polygon.has_value() && current_polygon.has_value()) {
+        if (polygon.value().id() != current_polygon.value().id()) {
+          current_polygon = polygon;
+          current_polygon_border_indices.clear();
+          current_polygon_border_indices.push_back(
+            get_corresponding_polygon_index(current_polygon.value(), bound_point.id()));
+          continue;
+        }
+      }
+
       current_polygon = std::nullopt;
       current_polygon_border_indices.clear();
     }