fix(behavior velocity planner): occlusion spot slice

perception/traffic_light_map_based_detector/src/node.cpp

@@ -103,7 +103,8 @@ MapBasedDetector::MapBasedDetector(const rclcpp::NodeOptions & node_options)
     "~/input/camera_info", rclcpp::SensorDataQoS(),
     std::bind(&MapBasedDetector::cameraInfoCallback, this, _1));
   route_sub_ = create_subscription<autoware_auto_planning_msgs::msg::HADMapRoute>(
-    "~/input/route", 1, std::bind(&MapBasedDetector::routeCallback, this, _1));
+    "~/input/route", rclcpp::QoS{1}.transient_local(),
+    std::bind(&MapBasedDetector::routeCallback, this, _1));
 
   // publishers
   roi_pub_ = this->create_publisher<autoware_auto_perception_msgs::msg::TrafficLightRoiArray>(

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp

@@ -49,16 +49,12 @@ struct Slice
 // using the given range and desired slice length and width
 void buildSlices(
   std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range,
-  const double slice_length, const double slice_width);
-//!< @brief build an interpolated polygon between the given bounds
-void buildInterpolatedPolygon(
-  lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & from_bound,
-  const lanelet::BasicLineString2d & to_bound, const double from_length, const double to_length,
-  const double from_ratio_dist, const double to_ratio_dist);
+  const double slice_length, const double slice_width, const double resolution);
 //!< @brief build sidewalk slice from path
-std::vector<geometry::Slice> buildSidewalkSlices(
-  const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset,
-  const double lateral_offset, const double min_size, const double lateral_max_dist);
+void buildSidewalkSlices(
+  std::vector<geometry::Slice> & slice, const lanelet::ConstLanelet & path_lanelet,
+  const double longitudinal_offset, const double lateral_offset, const double min_size,
+  const double lateral_max_dist);
 //!< @brief calculate interpolation between a and b at distance ratio t
 template <typename T>
 T lerp(T a, T b, double t)

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -36,6 +36,7 @@
 
 namespace behavior_velocity_planner
 {
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
 namespace occlusion_spot_utils
 {
 enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN };

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp

@@ -194,6 +194,7 @@ visualization_msgs::msg::MarkerArray createMarkers(
     occlusion_spot_slowdown_markers.markers.insert(
       occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(),
       collision_markers.end());
+    break;
   }
 
   // draw slice if having sidewalk polygon

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp

@@ -24,76 +24,80 @@ namespace behavior_velocity_planner
 {
 namespace geometry
 {
+using lanelet::BasicLineString2d;
+using lanelet::BasicPoint2d;
+using lanelet::BasicPolygon2d;
+namespace bg = boost::geometry;
+namespace lg = lanelet::geometry;
+
+void createOffsetLineString(
+  const BasicLineString2d & in, const double offset, BasicLineString2d & offset_line_string)
+{
+  for (size_t i = 0; i < in.size() - 1; i++) {
+    const auto & p0 = in.at(i);
+    const auto & p1 = in.at(i + 1);
+    // translation
+    const double dy = p1[1] - p0[1];
+    const double dx = p1[0] - p0[0];
+    // rotation (use inverse matrix of rotation)
+    const double yaw = std::atan2(dy, dx);
+    // translation
+    const double offset_x = p0[0] - std::sin(yaw) * offset;
+    const double offset_y = p0[1] + std::cos(yaw) * offset;
+    offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y});
+    //! insert final offset linestring using prev vertical direction
+    if (i == in.size() - 2) {
+      const double offset_x = p1[0] - std::sin(yaw) * offset;
+      const double offset_y = p1[1] + std::cos(yaw) * offset;
+      offset_line_string.emplace_back(BasicPoint2d{offset_x, offset_y});
+    }
+  }
+  return;
+}
+
 void buildSlices(
   std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range,
-  const double slice_length, const double slice_width)
+  const double slice_length, const double slice_width, const double resolution)
 {
-  lanelet::BasicLineString2d path_boundary = path_lanelet.centerline2d().basicLineString();
-  if (path_boundary.size() < 2) {
-    return;
-  }
   const int num_lateral_slice =
     static_cast<int>(std::abs(range.max_distance - range.min_distance) / slice_width);
-  // ignore the last point
-  const int num_longitudinal_slice =
-    static_cast<int>(std::abs(range.max_length - range.min_length) / slice_length);
-  slices.reserve(num_lateral_slice * num_longitudinal_slice);
-  // Note: offsetNoThrow is necessary
-  // as the sharp turn at the end of the trajectory can "reverse" the linestring
   /**
-   * @brief
-   * build slice from occupancy grid : the first slice to create is ssss
-   *
-   *         | 0 | 1 | 2 | 3 | 4 |
-   *       0 | --|---|---|---|---|--  outer bound
-   *       1 |   | ? | ? |   |   |
-   *       2 |   | ? | ? |   |   |
-   *       3 |   | ? | ? |   |   |                  ^
-   *       4 | s | s | ? |   |   |                  | dist_ratio
-   *       5 |-s-|-s-|---|---|---|--  inner bound     --> length ratio
-   *       Ego--------------------->     total_slice_length
+   * @brief bounds
+   * +---------- outer bounds
+   * |   +------ inner bounds(original path)
+   * |   |
    */
-  lanelet::BasicLineString2d inner_bounds = path_boundary;
-  lanelet::BasicLineString2d outer_bounds;
-  outer_bounds = lanelet::geometry::offsetNoThrow(path_boundary, range.max_distance);
-  // correct self crossing with best effort
-  boost::geometry::correct(outer_bounds);
-  rclcpp::Clock clock{RCL_ROS_TIME};
-  try {
-    // if correct couldn't solve self crossing skip this area
-    lanelet::geometry::internal::checkForInversion(
-      path_boundary, outer_bounds, std::abs(range.max_distance));
-  } catch (...) {
-    RCLCPP_DEBUG_STREAM_THROTTLE(
-      rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"), clock, 5000,
-      "self crossing with offset " << range.max_distance);
-  }
-  // offset last point is especially messy
-  inner_bounds.pop_back();
-  outer_bounds.pop_back();
-  // resize to the same size as slice
-  inner_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size()));
-  outer_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size()));
-  if (inner_bounds.size() < 2 || outer_bounds.size() < 2) {
-    return;
-  }
+  BasicLineString2d inner_bounds = path_lanelet.centerline2d().basicLineString();
+  BasicLineString2d outer_bounds;
+  if (inner_bounds.size() < 2) return;
+  createOffsetLineString(inner_bounds, range.max_distance, outer_bounds);
   const double ratio_dist_start = std::abs(range.min_distance / range.max_distance);
   const double ratio_dist_increment = std::min(1.0, slice_width / std::abs(range.max_distance));
-  for (int s = 0; s < num_longitudinal_slice; s++) {
-    const double length = range.min_length + s * slice_length;
-    const double next_length = range.min_length + (s + 1.0) * slice_length;
-    const double min_length =
-      std::min(lanelet::geometry::length(outer_bounds), lanelet::geometry::length(inner_bounds));
-    if (next_length > min_length) {
-      break;
-    }
+  lanelet::BasicPolygon2d poly;
+  const int num_step = static_cast<int>(slice_length / resolution);
+  //! max index is the last index of path point
+  const int max_index = static_cast<int>(inner_bounds.size() - 1);
+  for (int s = 0; s < max_index; s += num_step) {
+    const double length = s * slice_length;
+    const double next_length = (s + num_step) * resolution;
     for (int d = 0; d < num_lateral_slice; d++) {
       const double ratio_dist = ratio_dist_start + d * ratio_dist_increment;
       const double next_ratio_dist = ratio_dist_start + (d + 1.0) * ratio_dist_increment;
       Slice slice;
-      buildInterpolatedPolygon(
-        slice.polygon, inner_bounds, outer_bounds, length, next_length, ratio_dist,
-        next_ratio_dist);
+      BasicLineString2d inner_polygons;
+      BasicLineString2d outer_polygons;
+      // build interpolated polygon for lateral
+      for (int i = 0; i <= num_step; i++) {
+        if (s + i >= max_index) continue;
+        inner_polygons.emplace_back(
+          lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), ratio_dist));
+        outer_polygons.emplace_back(
+          lerp(inner_bounds.at(s + i), outer_bounds.at(s + i), next_ratio_dist));
+      }
+      // Build polygon
+      inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend());
+      slice.polygon = lanelet::BasicPolygon2d(inner_polygons);
+      // add range info
       slice.range.min_length = length;
       slice.range.max_length = next_length;
       slice.range.min_distance = ratio_dist * range.max_distance;
@@ -103,106 +107,29 @@ void buildSlices(
   }
 }
 
-void buildInterpolatedPolygon(
-  lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & inner_bounds,
-  const lanelet::BasicLineString2d & outer_bounds, const double current_length,
-  const double next_length, const double from_ratio_dist, const double to_ratio_dist)
+void buildSidewalkSlices(
+  std::vector<Slice> & slices, const lanelet::ConstLanelet & path_lanelet,
+  const double longitudinal_offset, const double lateral_offset, const double slice_size,
+  const double lateral_max_dist)
 {
-  if (current_length >= next_length) {
-    RCLCPP_WARN(
-      rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"),
-      "buildInterpolatedPolygon: starting length must be lower than target length");
-  }
-  lanelet::BasicLineString2d inner_polygons;
-  lanelet::BasicLineString2d outer_polygons;
-  inner_polygons.reserve(inner_bounds.size());
-  outer_polygons.reserve(outer_bounds.size());
-  // Find starting point. Interpolate it if necessary
-  double length = 0.0;
-  size_t point_index = 0;
-  lanelet::BasicPoint2d inner_polygon_from;
-  lanelet::BasicPoint2d inner_polygon_to;
-  lanelet::BasicPoint2d outer_polygon_from;
-  lanelet::BasicPoint2d outer_polygon_to;
-  // Search first points of polygon
-  for (; length < current_length && point_index < inner_bounds.size() - 1; ++point_index) {
-    length +=
-      lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]);
-  }
-  // if find current bound point index
-  if (length > current_length) {
-    const double length_between_points =
-      lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]);
-    const double length_ratio =
-      (length_between_points - (length - current_length)) / length_between_points;
-    inner_polygon_from =
-      lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio);
-    outer_polygon_from =
-      lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio);
-  } else {
-    inner_polygon_from = inner_bounds[point_index];
-    outer_polygon_from = outer_bounds[point_index];
-    if (length < next_length && point_index < inner_bounds.size() - 1) {
-      length +=
-        lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]);
-      ++point_index;
-    }
-  }
-  inner_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, from_ratio_dist));
-  outer_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, to_ratio_dist));
-
-  // Intermediate points
-  for (; length < next_length && point_index < inner_bounds.size() - 1; ++point_index) {
-    inner_polygons.emplace_back(
-      lerp(inner_bounds[point_index], outer_bounds[point_index], from_ratio_dist));
-    outer_polygons.emplace_back(
-      lerp(inner_bounds[point_index], outer_bounds[point_index], to_ratio_dist));
-    length +=
-      lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]);
-  }
-  // Last points
-  if (length > next_length) {
-    const double length_between_points =
-      lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]);
-    const double length_ratio =
-      (length_between_points - (length - next_length)) / length_between_points;
-    inner_polygon_to = lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio);
-    outer_polygon_to = lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio);
-  } else {
-    inner_polygon_to = inner_bounds[point_index];
-    outer_polygon_to = outer_bounds[point_index];
-  }
-  inner_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, from_ratio_dist));
-  outer_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, to_ratio_dist));
-  // Build polygon
-  inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend());
-  polygons = lanelet::BasicPolygon2d(inner_polygons);
-}
-
-std::vector<geometry::Slice> buildSidewalkSlices(
-  const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset,
-  const double lateral_offset, const double slice_size, const double lateral_max_dist)
-{
-  std::vector<geometry::Slice> slices;
-  std::vector<geometry::Slice> left_slices;
-  std::vector<geometry::Slice> right_slices;
-  const double longitudinal_max_dist = lanelet::geometry::length2d(path_lanelet);
-  geometry::SliceRange left_slice_range = {
+  std::vector<Slice> left_slices;
+  std::vector<Slice> right_slices;
+  const double longitudinal_max_dist = lg::length2d(path_lanelet);
+  SliceRange left_slice_range = {
     longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist};
-  geometry::buildSlices(left_slices, path_lanelet, left_slice_range, slice_size, slice_size);
-  geometry::SliceRange right_slice_range = {
+  // in most case lateral distance is much more effective for velocity planning
+  const double slice_length = 4.0 * slice_size;
+  const double slice_width = slice_size;
+  const double resolution = 1.0;
+  buildSlices(left_slices, path_lanelet, left_slice_range, slice_length, slice_width, resolution);
+  SliceRange right_slice_range = {
     longitudinal_offset, longitudinal_max_dist, -lateral_offset,
     -lateral_offset - lateral_max_dist};
-  geometry::buildSlices(right_slices, path_lanelet, right_slice_range, slice_size, slice_size);
+  buildSlices(right_slices, path_lanelet, right_slice_range, slice_length, slice_width, resolution);
   // Properly order lanelets from closest to furthest
-  for (size_t i = 0; i < right_slices.size(); ++i) {
-    slices.emplace_back(right_slices[i]);
-  }
-  for (size_t i = 0; i < left_slices.size(); ++i) {
-    slices.emplace_back(left_slices[i]);
-  }
-
-  return slices;
+  slices = left_slices;
+  slices.insert(slices.end(), right_slices.begin(), right_slices.end());
+  return;
 }
 }  // namespace geometry
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp

@@ -32,6 +32,20 @@ namespace behavior_velocity_planner
 {
 namespace occlusion_spot_utils
 {
+lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path)
+{
+  lanelet::Points3d path_points;
+  path_points.reserve(path.points.size());
+  for (const auto & path_point : path.points) {
+    const auto & p = path_point.point.pose.position;
+    path_points.emplace_back(lanelet::InvalId, p.x, p.y, p.z);
+  }
+  lanelet::LineString3d centerline(lanelet::InvalId, path_points);
+  lanelet::Lanelet path_lanelet(lanelet::InvalId);
+  path_lanelet.setCenterline(centerline);
+  return lanelet::ConstLanelet(path_lanelet);
+}
+
 bool splineInterpolate(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
   autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger)
@@ -419,7 +433,7 @@ void generatePossibleCollisions(
   const PlannerParam & param, std::vector<lanelet::BasicPolygon2d> & debug)
 {
   // NOTE : buildPathLanelet first index should always be zero because path is already limited
-  lanelet::ConstLanelet path_lanelet = buildPathLanelet(path);
+  lanelet::ConstLanelet path_lanelet = toPathLanelet(path);
   if (path_lanelet.centerline2d().empty()) {
     return;
   }
@@ -446,9 +460,10 @@ void generateSidewalkPossibleCollisions(
   } else {
     min_range = param.vehicle_info.baselink_to_front - offset_form_ego_to_target;
   }
-  std::vector<geometry::Slice> sidewalk_slices = geometry::buildSidewalkSlices(
-    path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5, param.sidewalk.slice_size,
-    param.sidewalk.focus_range);
+  std::vector<geometry::Slice> sidewalk_slices;
+  geometry::buildSidewalkSlices(
+    sidewalk_slices, path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5,
+    param.sidewalk.slice_size, param.sidewalk.focus_range);
   double length_lower_bound = std::numeric_limits<double>::max();
   double distance_lower_bound = std::numeric_limits<double>::max();
   std::sort(