feat(intersection, blind_spot): add objects of interest marker

planning/behavior_velocity_blind_spot_module/src/scene.cpp

@@ -348,67 +348,69 @@
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-  const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const
+  const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose)
 {
   /* get detection area */
   if (turn_direction_ == TurnDirection::INVALID) {
     RCLCPP_WARN(logger_, "blind spot detector is running, turn_direction_ = not right or left.");
     return false;
   }
 
   const auto areas_opt = generateBlindSpotPolygons(
     lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose);
   if (areas_opt) {
     const auto & detection_areas = areas_opt.value().detection_areas;
     const auto & conflict_areas = areas_opt.value().conflict_areas;
     const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas;
     const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas;
     debug_data_.detection_areas = detection_areas;
     debug_data_.conflict_areas = conflict_areas;
     debug_data_.detection_areas.insert(
       debug_data_.detection_areas.end(), opposite_detection_areas.begin(),
       opposite_detection_areas.end());
     debug_data_.conflict_areas.insert(
       debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(),
       opposite_conflict_areas.end());
 
     autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr;
     cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time);
 
     // check objects in blind spot areas
     for (const auto & object : objects.objects) {
       if (!isTargetObjectType(object)) {
         continue;
       }
 
       // right direction
       const bool exist_in_right_detection_area =
         std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) {
           return bg::within(
             to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
             lanelet::utils::to2D(area));
         });
       // opposite direction
       const bool exist_in_opposite_detection_area = std::any_of(
         opposite_detection_areas.begin(), opposite_detection_areas.end(),
         [&object](const auto & area) {
           return bg::within(
             to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position),
             lanelet::utils::to2D(area));
         });
       const bool exist_in_detection_area =
         exist_in_right_detection_area || exist_in_opposite_detection_area;
       if (!exist_in_detection_area) {
         continue;
       }
       const bool exist_in_right_conflict_area =
         isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose);
       const bool exist_in_opposite_conflict_area = isPredictedPathInArea(
         object, opposite_conflict_areas, planner_data_->current_odometry->pose);
       const bool exist_in_conflict_area =
         exist_in_right_conflict_area || exist_in_opposite_conflict_area;
       if (exist_in_detection_area && exist_in_conflict_area) {
         debug_data_.conflicting_targets.objects.push_back(object);
+        setObjectsOfInterestData(
+          object.kinematics.initial_pose_with_covariance.pose, object.shape, ColorName::RED);
         return true;
       }
     }

planning/behavior_velocity_blind_spot_module/src/scene.hpp

@@ -113,7 +113,7 @@ class BlindSpotModule : public SceneModuleInterface
     lanelet::routing::RoutingGraphPtr routing_graph_ptr,
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr,
-    const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const;
+    const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose);
 
   /**
    * @brief Create half lanelet

planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp

@@ -621,6 +621,9 @@
       continue;
     }
     const auto & unsafe_info = object_info->is_unsafe().value();
+    setObjectsOfInterestData(
+      object_info->predicted_object().kinematics.initial_pose_with_covariance.pose,
+      object_info->predicted_object().shape, ColorName::RED);
     // ==========================================================================================
     // if ego is over the pass judge lines, then the visualization as "too_late_objects" or
     // "misjudge_objects" is more important than that for "unsafe"
@@ -910,7 +913,7 @@
         }
       }
       object_ttc_time_array.layout.dim.at(0).size++;
-      const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position;
+      const auto & pos = object..position;
       const auto & shape = object.shape;
       object_ttc_time_array.data.insert(
         object_ttc_time_array.data.end(),