adding formal comment

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -193,10 +193,6 @@ std::optional<size_t> getFirstPointInsidePolygon(
   return std::nullopt;
 }
 
-/**
- * @param pair lanelets and the vector of original lanelets in topological order (not reversed as
- *in generateDetectionLaneDivisions())
- **/
 std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
 mergeLaneletsByTopologicalSort(
   const lanelet::ConstLanelets & lanelets,
@@ -314,74 +310,6 @@ bool isBeforeTargetIndex(
   return static_cast<bool>(target_idx > closest_idx);
 }
 
-/*
-static std::vector<int> getAllAdjacentLanelets(
-  const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane)
-{
-  std::set<int> results;
-
-  results.insert(lane.id());
-
-  auto it = routing_graph->adjacentRight(lane);
-  // take all lane on the right side
-  while (!!it) {
-    results.insert(it.get().id());
-    it = routing_graph->adjacentRight(it.get());
-  }
-  // take all lane on the left side
-  it = routing_graph->adjacentLeft(lane);
-  while (!!it) {
-    results.insert(it.get().id());
-    it = routing_graph->adjacentLeft(it.get());
-
-  }
-  return std::vector<int>(results.begin(), results.end());
-}
-*/
-
-/*
-lanelet::ConstLanelets extendedAdjacentDirectionLanes(
-  lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph,
-  lanelet::ConstLanelet lane)
-{
-  // some of the intersections are not well-formed, and "adjacent" turning
-  // lanelets are not sharing the LineStrings
-  const std::string turn_direction = getTurnDirection(lane);
-  if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight")
-    return {};
-
-  std::set<int> previous_lanelet_ids;
-  for (auto && previous_lanelet : routing_graph->previous(lane)) {
-    previous_lanelet_ids.insert(previous_lanelet.id());
-  }
-
-  std::set<int> besides_previous_lanelet_ids;
-  for (auto && previous_lanelet_id : previous_lanelet_ids) {
-    lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id);
-    for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) {
-      besides_previous_lanelet_ids.insert(beside_lanelet);
-    }
-  }
-
-  std::set<int> following_turning_lanelets;
-  following_turning_lanelets.insert(lane.id());
-  for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) {
-    lanelet::ConstLanelet besides_previous_lanelet =
-      map->laneletLayer.get(besides_previous_lanelet_id);
-    for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) {
-      // if this has {"turn_direction", "${turn_direction}"}, take this
-      if (getTurnDirection(following_lanelet) == turn_direction)
-        following_turning_lanelets.insert(following_lanelet.id());
-    }
-  }
-  lanelet::ConstLanelets ret{};
-  for (auto && id : following_turning_lanelets) {
-    ret.push_back(map->laneletLayer.get(id));
-  }
-  return ret;
-}
-*/
-
 std::optional<tier4_autoware_utils::Polygon2d> getIntersectionArea(
   lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr)
 {
@@ -422,7 +350,6 @@ std::optional<InterpolatedPathInfo> generateInterpolatedPath(
   return interpolated_path_info;
 }
 
-// from here
 geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection(
   const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state)
 {

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -54,9 +54,9 @@ std::optional<size_t> getFirstPointInsidePolygon(
 
 /**
  * @brief check if ego is over the target_idx. If the index is same, compare the exact pose
- * @param path path
- * @param closest_idx ego's closest index on the path
- * @param current_pose ego's exact pose
+ * @param[in] path path
+ * @param[in] closest_idx ego's closest index on the path
+ * @param[in] current_pose ego's exact pose
  * @return true if ego is over the target_idx
  */
 bool isOverTargetIndex(
@@ -67,12 +67,6 @@ bool isBeforeTargetIndex(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx,
   const geometry_msgs::msg::Pose & current_pose, const int target_idx);
 
-/*
-lanelet::ConstLanelets extendedAdjacentDirectionLanes(
-lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph,
-lanelet::ConstLanelet lane);
-*/
-
 std::optional<tier4_autoware_utils::Polygon2d> getIntersectionArea(
   lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr);
 
@@ -86,6 +80,15 @@ std::optional<InterpolatedPathInfo> generateInterpolatedPath(
 geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection(
   const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state);
 
+/**
+ * @fn
+ * @brief This function sorts the set of lanelets topologically using topological sort and merges
+ * the lanelets from each root to each end. Each branch is merged and returned with the original
+ * lanelets
+ * @param[in] lanelets the set of lanelets
+ * @param[in] routing_graph_ptr the routing graph
+ * @return the pair of merged lanelets and their corresponding original lanelets
+ */
 std::pair<lanelet::ConstLanelets, std::vector<lanelet::ConstLanelets>>
 mergeLaneletsByTopologicalSort(
   const lanelet::ConstLanelets & lanelets,

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -34,12 +34,18 @@
 namespace behavior_velocity_planner::util
 {
 
+/** wrapper class of interpolated path with lane id*/
 struct InterpolatedPathInfo
 {
+  /** the interpolated path */
   autoware_auto_planning_msgs::msg::PathWithLaneId path;
+  /** discretization interval of interpolation */
   double ds{0.0};
+  /** the intersection lanelet id */
   lanelet::Id lane_id{0};
+  /** the associative lane ids of lane_id */
   std::set<lanelet::Id> associative_lane_ids{};
+  /** the range of indices for the path points with associative lane id */
   std::optional<std::pair<size_t, size_t>> lane_id_interval{std::nullopt};
 };