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

…arefoundation#6201) Signed-off-by: Mamoru Sobue <[email protected]>
tier4 · Feb 6, 2024 · f9ceb42 · f9ceb42
1 parent 2689c50
commit f9ceb42
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 3 deletions.
diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp
@@ -348,7 +348,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
   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) {
@@ -409,6 +409,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot(
         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;
       }
     }

diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/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

diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp
@@ -621,6 +621,9 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision(
       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 @@ bool IntersectionModule::checkCollision(
         }
       }
       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(),