fix(intersection): fix uninitialized tl_arrow_info variable issue (au…

…towarefoundation#4283)

* update first conflicting/attention area

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

* debug 1113, 1393

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

* migrated to latest

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

---------

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

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -650,27 +650,25 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
   }
 
   const auto & current_pose = planner_data_->current_odometry->pose;
-  // dynamically change detection area based on tl_arrow_solid_on
-  const bool tl_arrow_solid_on =
-    util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
-  if (
-    !intersection_lanelets_ ||
-    intersection_lanelets_.value().tl_arrow_solid_on != tl_arrow_solid_on) {
+  if (!intersection_lanelets_) {
     const auto lanelets_on_path =
       planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose);
     intersection_lanelets_ = util::getObjectiveLanelets(
       lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_,
-      interpolated_path_info, planner_param_.common.attention_area_length, tl_arrow_solid_on);
+      planner_param_.common.attention_area_length,
+      planner_param_.occlusion.occlusion_attention_area_length);
   }
+  const bool tl_arrow_solid_on =
+    util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map);
+  intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info);
 
-  const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting;
-  const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area;
+  const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting();
+  const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area();
   if (conflicting_lanelets.empty() || !first_conflicting_area) {
-    RCLCPP_DEBUG(logger_, "conflicting area is empty");
     return IntersectionModule::Indecisive{};
   }
 
-  const auto & first_attention_area = intersection_lanelets_.value().first_attention_area;
+  const auto & first_attention_area = intersection_lanelets_.value().first_attention_area();
   const auto & dummy_first_attention_area =
     first_attention_area ? first_attention_area.value() : first_conflicting_area.value();
   const auto intersection_stop_lines_opt = util::generateIntersectionStopLines(
@@ -738,13 +736,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
     return IntersectionModule::Indecisive{};
   }
 
-  const auto & attention_lanelets = intersection_lanelets_.value().attention;
-  const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent;
-  const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention;
-  const auto & attention_area = intersection_lanelets_.value().attention_area;
-  const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area;
-  debug_data_.attention_area = attention_area;
-  debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area;
+  const auto & attention_lanelets = intersection_lanelets_.value().attention();
+  const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent();
+  const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention();
+  const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area();
+  debug_data_.attention_area = intersection_lanelets_.value().attention_area();
+  debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area();
 
   // get intersection area
   const auto intersection_area = planner_param_.common.use_intersection_area

planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp

@@ -83,17 +83,17 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR
   }
 
   /* get detection area */
-  if (!intersection_lanelets_.has_value()) {
+  if (!intersection_lanelets_) {
     const auto & assigned_lanelet =
       planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_);
     const auto lanelets_on_path = planning_utils::getLaneletsOnPath(
       *path, lanelet_map_ptr, planner_data_->current_odometry->pose);
     intersection_lanelets_ = util::getObjectiveLanelets(
       lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_,
-      interpolated_path_info, planner_param_.attention_area_length,
-      false /* tl_arrow_solid on does not matter here*/);
+      planner_param_.attention_area_length, 0.0);
   }
-  const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area;
+  intersection_lanelets_.value().update(false, interpolated_path_info);
+  const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area();
   if (!first_conflicting_area) {
     return false;
   }

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -14,6 +14,8 @@
 
 #include "util.hpp"
 
+#include "util_type.hpp"
+
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
@@ -400,9 +402,8 @@ static std::vector<lanelet::CompoundPolygon3d> getPolygon3dFromLanelets(
 IntersectionLanelets getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path,
-  const std::set<int> & associative_ids, const InterpolatedPathInfo & interpolated_path_info,
-  const double detection_area_length, const double occlusion_detection_area_length,
-  const bool tl_arrow_solid_on)
+  const std::set<int> & associative_ids, const double detection_area_length,
+  const double occlusion_detection_area_length)
 {
   const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else");
 
@@ -472,9 +473,8 @@ IntersectionLanelets getObjectiveLanelets(
   }
 
   // get possible lanelet path that reaches conflicting_lane longer than given length
-  // if traffic light arrow is active, this process is unnecessary
   lanelet::ConstLanelets detection_and_preceding_lanelets;
-  if (!tl_arrow_solid_on) {
+  {
     const double length = detection_area_length;
     std::set<lanelet::Id> detection_ids;
     for (const auto & ll : detection_lanelets) {
@@ -516,36 +516,16 @@ IntersectionLanelets getObjectiveLanelets(
   }
 
   IntersectionLanelets result;
-  if (!tl_arrow_solid_on) {
-    result.attention = std::move(detection_and_preceding_lanelets);
-  } else {
-    result.attention = std::move(detection_lanelets);
-  }
-  result.conflicting = std::move(conflicting_ex_ego_lanelets);
-  result.adjacent = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids);
-  result.occlusion_attention = std::move(occlusion_detection_and_preceding_lanelets);
+  result.attention_ = std::move(detection_and_preceding_lanelets);
+  result.attention_non_preceding_ = std::move(detection_lanelets);
+  result.conflicting_ = std::move(conflicting_ex_ego_lanelets);
+  result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids);
+  result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets);
   // compoundPolygon3d
-  result.attention_area = getPolygon3dFromLanelets(result.attention);
-  result.conflicting_area = getPolygon3dFromLanelets(result.conflicting);
-  result.adjacent_area = getPolygon3dFromLanelets(result.adjacent);
-  result.occlusion_attention_area = getPolygon3dFromLanelets(result.occlusion_attention);
-
-  // find the first conflicting/detection area polygon intersecting the path
-  const auto & path = interpolated_path_info.path;
-  const auto & lane_interval = interpolated_path_info.lane_id_interval.value();
-  {
-    auto first = getFirstPointInsidePolygons(path, lane_interval, result.conflicting_area);
-    if (first) {
-      result.first_conflicting_area = first.value().second;
-    }
-  }
-  {
-    auto first = getFirstPointInsidePolygons(path, lane_interval, result.attention_area);
-    if (first) {
-      result.first_attention_area = first.value().second;
-    }
-  }
-
+  result.attention_area_ = getPolygon3dFromLanelets(result.attention_);
+  result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_);
+  result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_);
+  result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_);
   return result;
 }
 
@@ -1099,5 +1079,26 @@ double calcDistanceUntilIntersectionLanelet(
   return distance;
 }
 
+void IntersectionLanelets::update(
+  const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info)
+{
+  tl_arrow_solid_on_ = tl_arrow_solid_on;
+  // find the first conflicting/detection area polygon intersecting the path
+  const auto & path = interpolated_path_info.path;
+  const auto & lane_interval = interpolated_path_info.lane_id_interval.value();
+  {
+    auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_);
+    if (first) {
+      first_conflicting_area_ = first.value().second;
+    }
+  }
+  {
+    auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_);
+    if (first) {
+      first_attention_area_ = first.value().second;
+    }
+  }
+}
+
 }  // namespace util
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -56,9 +56,8 @@ std::optional<std::pair<size_t, size_t>> findLaneIdsInterval(
 IntersectionLanelets getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path,
-  const std::set<int> & associative_ids, const InterpolatedPathInfo & interpolated_path_info,
-  const double detection_area_length, const double occlusion_detection_area_length,
-  const bool tl_arrow_solid_on = false);
+  const std::set<int> & associative_ids, const double detection_area_length,
+  const double occlusion_detection_area_length);
 
 /**
  * @brief Generate a stop line for stuck vehicle

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -51,22 +51,67 @@ struct DebugData
   std::optional<geometry_msgs::msg::Point> nearest_occlusion_projection_point = std::nullopt;
 };
 
+struct InterpolatedPathInfo
+{
+  autoware_auto_planning_msgs::msg::PathWithLaneId path;
+  double ds;
+  int lane_id;
+  std::set<int> associative_lane_ids;
+  std::optional<std::pair<size_t, size_t>> lane_id_interval;
+};
+
 struct IntersectionLanelets
 {
-  bool tl_arrow_solid_on;
-  lanelet::ConstLanelets attention;
-  lanelet::ConstLanelets conflicting;
-  lanelet::ConstLanelets adjacent;
-  lanelet::ConstLanelets occlusion_attention;  // for occlusion detection
-  std::vector<lanelet::CompoundPolygon3d> attention_area;
-  std::vector<lanelet::CompoundPolygon3d> conflicting_area;
-  std::vector<lanelet::CompoundPolygon3d> adjacent_area;
-  std::vector<lanelet::CompoundPolygon3d> occlusion_attention_area;
+public:
+  void update(const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info);
+  const lanelet::ConstLanelets & attention() const
+  {
+    return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_;
+  }
+  const lanelet::ConstLanelets & conflicting() const { return conflicting_; }
+  const lanelet::ConstLanelets & adjacent() const { return adjacent_; }
+  const lanelet::ConstLanelets & occlusion_attention() const
+  {
+    return tl_arrow_solid_on_ ? attention_non_preceding_ : occlusion_attention_;
+  }
+  const std::vector<lanelet::CompoundPolygon3d> & attention_area() const
+  {
+    return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_;
+  }
+  const std::vector<lanelet::CompoundPolygon3d> & conflicting_area() const
+  {
+    return conflicting_area_;
+  }
+  const std::vector<lanelet::CompoundPolygon3d> & adjacent_area() const { return adjacent_area_; }
+  const std::vector<lanelet::CompoundPolygon3d> & occlusion_attention_area() const
+  {
+    return occlusion_attention_area_;
+  }
+  const std::optional<lanelet::CompoundPolygon3d> & first_conflicting_area() const
+  {
+    return first_conflicting_area_;
+  }
+  const std::optional<lanelet::CompoundPolygon3d> & first_attention_area() const
+  {
+    return first_attention_area_;
+  }
+
+  lanelet::ConstLanelets attention_;
+  lanelet::ConstLanelets attention_non_preceding_;
+  lanelet::ConstLanelets conflicting_;
+  lanelet::ConstLanelets adjacent_;
+  lanelet::ConstLanelets occlusion_attention_;  // for occlusion detection
+  std::vector<lanelet::CompoundPolygon3d> attention_area_;
+  std::vector<lanelet::CompoundPolygon3d> attention_non_preceding_area_;
+  std::vector<lanelet::CompoundPolygon3d> conflicting_area_;
+  std::vector<lanelet::CompoundPolygon3d> adjacent_area_;
+  std::vector<lanelet::CompoundPolygon3d> occlusion_attention_area_;
   // the first area intersecting with the path
   // even if lane change/re-routing happened on the intersection, these areas area are supposed to
   // be invariant under the 'associative' lanes.
-  std::optional<lanelet::CompoundPolygon3d> first_conflicting_area;
-  std::optional<lanelet::CompoundPolygon3d> first_attention_area;
+  bool tl_arrow_solid_on_ = false;
+  std::optional<lanelet::CompoundPolygon3d> first_conflicting_area_ = std::nullopt;
+  std::optional<lanelet::CompoundPolygon3d> first_attention_area_ = std::nullopt;
 };
 
 struct DescritizedLane
@@ -76,15 +121,6 @@ struct DescritizedLane
   std::vector<lanelet::ConstLineString2d> divisions;
 };
 
-struct InterpolatedPathInfo
-{
-  autoware_auto_planning_msgs::msg::PathWithLaneId path;
-  double ds;
-  int lane_id;
-  std::set<int> associative_lane_ids;
-  std::optional<std::pair<size_t, size_t>> lane_id_interval;
-};
-
 struct IntersectionStopLines
 {
   // NOTE: for baselink